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

結果

ファイル:LINE MOVIE 1596614650003 small.mp4

詳細

PID制御

• (P:比例、I:積分、D:微分)

※0.5の値を忘れてしまった。（どれかのゲインだったはず）

1.0 * (現在の光センサー - ((黒の最大値+白の最小値)/2))*100 / (float)(黒の最大値-白の最小値)+0.5 * (現在の光センサー-１つ前の光センサー)/(float)(黒の最大値-白の最小値)*100

メソッドGOの流れ

1・引数に 0 , 1を渡す。これは学習済みかどうかを制御するために必要
  0:黒である最大値と白である最小値のおおよそ境界線を左にハンドルを切りながら測定(500回), 
  1:0のおおよその境界線を元に精密に学習し1万回後に白黒をPID式に反映
  (スタート時の黒と白の値がわからないため0で探す)

2・現在の光源の値を取得

3・PID制御でどのくらいspeedに対して減速・加速させるかの値を計算

4・引数の0と1で境界線を取得済みであるか制御する。
   (0であれば、黒の最大値と白の最小値を１回目×500回で取得する。)
   (1であれば、黒の最大値と白の最小値を１万回目計測後反映する。十分な値を録るように１万回目に設定)

5・4の取得の際にポートCとポートBのタイヤに対して減速・加速をspeedからPID値で出力する.

6・終了（繰り返し）

コード

/* 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);
}