C言語 etロボ ライン上をPID制御をする

コード

/* helloworld.c for TOPPERS/ATK(OSEK) */
#include "kernel.h"
#include "kernel_id.h"
#include "stdio.h"
#include "stdlib.h"
#include "math.h"
#include "ecrobot_interface.h"

DeclareTask(OSEK_Task_Background);			/* Task1を宣言 */

void main_task(void);
void go(int ques); //両タイヤの状態確認と開始
void stop(void);
void wait_times(int time);
static int course;
static int volume = 50;
static int state;
static int system_ok = 0;
static int mb = 1;
static int max_B = 600;
static int min_W = 400;
static int max_B_tmp = 600;
static int min_W_tmp = 400;
static int studey_turn = 1;

void ecrobot_device_initialize(){		/* OSEK起動時の処理（モータ停止）*/
	nxt_motor_set_speed(NXT_PORT_B,0,1);
	nxt_motor_set_speed(NXT_PORT_C,0,1);
	nxt_motor_set_count(NXT_PORT_C,0);
	nxt_motor_set_count(NXT_PORT_B,0);
	ecrobot_init_sonar_sensor(NXT_PORT_S4);
	ecrobot_set_light_sensor_active(NXT_PORT_S3);
	ecrobot_init_bt_connection();
}

void ecrobot_device_terminate(){		/* OSEK終了時の処理（モータ停止）*/
	nxt_motor_set_speed(NXT_PORT_B,0,1);
	nxt_motor_set_speed(NXT_PORT_C,0,1);
	nxt_motor_set_count(NXT_PORT_C,0);
	nxt_motor_set_count(NXT_PORT_B,0);
	ecrobot_term_sonar_sensor(NXT_PORT_S4);
	ecrobot_set_light_sensor_inactive(NXT_PORT_S3);
	ecrobot_term_bt_connection();
}


/* nxtOSEK hook to be invoked from an ISR in category 2 */
void user_1ms_isr_type2(void){ }


TASK(OSEK_Task_Background)
{
    ecrobot_device_initialize();
    display_clear(1);
    systick_wait_ms(2000);
		while(1){
            main_task(); //ある一定間で呼び出されるタスク

            if(system_ok == 1){ // 脱出
            	ecrobot_device_terminate();
            	ecrobot_sound_tone(880,250,(volume-40));
        		systick_wait_ms(250);
                break;
            }
        }
	ecrobot_device_terminate();
	TerminateTask();
}
void main_task(void){

	/*state*/
    //必要な状態を数学に割り当てる
    #define STATE_RUNNING 0  //走っている状態
    #define STATE_END 21 //

	/*course*/
	#define STUDY 2
	#define nxt_STRAIGHT 3

    static float encoder = 0.0f; //モータのエンコーダ（回転角度）の値
    if(mb <= 1){

    	state = STATE_RUNNING; //自分の状態を保持しておく変数
    	course = STUDY;
    	mb = 2;

	}

    switch(state){

        case STATE_RUNNING:

        	switch(course){

        		case STUDY:

        			go(0);
        			encoder += 1;
        			if(encoder > 500){
        			    course = nxt_STRAIGHT;
        			    break;
        			}

        			break;

        	    case nxt_STRAIGHT:

        	    	go(1);
        	    	break;

        	}

        	break;

        case STATE_END:
        	ecrobot_sound_tone(980,100,(volume-40));
        	stop();
        	system_ok = 1;
            break;

    }

}

void wait_times(int time){
	int count_time = 0;
	int dis_time = 0;
	while (time > count_time){
		dis_time = ecrobot_get_sonar_sensor(NXT_PORT_S4);
		systick_wait_ms(time);
		count_time += time;
		if(((dis_time < 35) && (dis_time > 0)) && (dis_time != -1)){
			state = STATE_END;
			system_ok = 1;
			break;
		}else if((count_time > time)){
			break;
		}
	}
}

void go(int ques){

	int light_count = ecrobot_get_light_sensor(NXT_PORT_S3);
	int light_tmp = light_count;
	int speed = 95; //88
	wait_times(7);
	//PID制御
	float turn = 1.0 * (light_count - ((max_B+min_W)/2))*100 / (float)(max_B-min_W)+0.5 * (light_count-light_tmp)/(float)(max_B-min_W)*100;
	if(ques == 0){
		nxt_motor_set_speed(NXT_PORT_C,((int)(speed-turn)),1);
		nxt_motor_set_speed(NXT_PORT_B,((int)(turn+speed)),1);
		if(min_W_tmp > light_count){
			min_W_tmp = light_count;
		}
		if(max_B_tmp < light_count){
			max_B_tmp = light_count;
		}
	}else{
		if(studey_turn == 1){
			max_B = max_B_tmp;
			min_W = min_W_tmp;
			studey_turn = 2;
		}
		nxt_motor_set_speed(NXT_PORT_C,((int)(speed-turn)),1);
		nxt_motor_set_speed(NXT_PORT_B,((int)(turn+speed)),1);
		studey_turn += 1;
		if(min_W_tmp > light_count){
			min_W_tmp = light_count;
		}
		if(max_B_tmp < light_count){
			max_B_tmp = light_count;
		}
		if(studey_turn >= 10000){
			studey_turn = 1;
		}
	}
	light_tmp = light_count;
}
void stop(void){
	ecrobot_sound_tone(1000,50,volume-70);
    nxt_motor_set_speed(NXT_PORT_B,0,0);
    nxt_motor_set_speed(NXT_PORT_C,0,0);
}