const int speed[8] = { 0 , 0x50 , 0x80 , 0xa0 , 0xb0 , 0xc0 , 0xd0 , 0xe0 }; //モーターの速度を更新 void motor(int left,int right){ set_motor(0,left); set_motor(1,right); } //PID制御 float exp_motorpow(int sonsor_val ,int target_val ){ #define DELTA_T 0.04 //ここは自分で指定 #define KP 0.0 #define KI 0.0 #define KD 0.0 float p,i,d; float ret; static float diff[2]; diff[0] = diff[1]; diff[1] = sonsor_val - target_val; p = KP*diff[1]; i = 0; d = KD*(diff[1]-diff[0])/DELTA_T; ret = p+i+d; if( ret > 3 )ret= 3; if( ret <-3 )ret=-3; return ret; } //実際の画像解析 void image_task(intptr_t exinf) { _UINT size = 0; int i,j; int x,y; int r,g,b; int sum_h; int sum_s; int sum_v; int sum_cnt; Hsv hsv; unsigned short *gra_vram; int count =0; unsigned long cx,cy; unsigned int colorcount; int val; //カメラのデータのRAM上のアドレス gra_vram = (unsigned short *)CAM_RAM_ADDRESS; while(1){ cx=0; cy=0; colorcount=0; //ライン検出 for(y=230;y<235;y++){ for(x=50;x<352-50;x++){ r = *(gra_vram+ y *352 + x); g = getg(r); b = getb(r); r = getr(r); //黒であるか if( r<0x50 && g<0x50 && b<0x50 ){ cx+=x; colorcount++; } } } //黒い部分があったなら if(colorcount>0){ val = cx/colorcount; //黒い部分の中心を求める val = (val-176); //「中心に対してどれだけずれているか」に変換 val = exp_motorpow( val , 0 ); //PID制御(PDのみ使用) motor( speed[3+val] , speed[3-val] ); //モーターに出力 } else{ /*画面上にラインが映っていないときの処理 何も書いていない状態だと線が見えなくなる直前の動きを継続する。 鋭角カーブでさえなければこれでもうまくいくはず。 しかし、線切れがあるフィールドでは要注意 */ } dly_tsk(1000/30); count++; } ext_tsk(); }