C言語 etロボライン上をPID制御をするのソースを表示

== 詳細 ==
=== PID制御 ===
 1.0 * (現在の光センサー - ((黒の最大値+白の最小値)/2))*100 / (float)(黒の最大値-白の最小値)+0.5 * (現在の光センサー-１つ前の光センサー)/(float)(黒の最大値-白の最小値)*100
=== メソッドGOの流れ ===
 1・引数に 0 , 1を渡す。これは学習済みかどうかを制御するために必要
   0:黒である最大値と白である最小値をおおよそ計測しPID式に反映(500回), 
   1:0のおおよその内容を元に精密に学習しPID式に反映
   (スタート時の黒と白の値がわからないため0でおおよその境界線を探す)
 2・現在の光源の値を取得
 3・PID制御でどのくらいspeedに対して減速・加速させるかの値を計算
    その際、２周目であれば学習済みの黒である最大値と白である最小値を使用しPIDを出す。
 4・引数の0と1で学習済みであるか制御する。
    (0であれば、黒の最大値と白の最小値を１回目で取得する。)
    (1であれば、黒の最大値と白の最小値を１万回目で、取得する。十分な値を録るように１万回目に設定)
 5・4の取得の際にポートCとポートBのタイヤに対して減速・加速をspeedからPID値で出力する.
 6・終了（繰り返し）

== コード ==
<syntaxhighlight lang="c">
/* 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);
}
</syntaxhighlight>