Takeshi Nishimura
/
MiniTracer2019
Linetrace&SensorvalueSort
Diff: main.cpp
- Revision:
- 1:c5854b9559f9
- Parent:
- 0:21afd0549d07
diff -r 21afd0549d07 -r c5854b9559f9 main.cpp --- a/main.cpp Sat Apr 28 06:26:41 2018 +0000 +++ b/main.cpp Sun Jun 09 07:46:22 2019 +0000 @@ -23,6 +23,7 @@ //割り込み定義 Ticker flipper; //汎用タイマー +Ticker sensget; //センサー用タイマー //プロトタイプ宣言 void init(void); //マイコン初期設定 @@ -32,6 +33,7 @@ void SensUp(void); //センサー値更新 int GetVol(void); //ボリューム値取得 + //グローバル変数の宣言 int timer1=0; //汎用タイマー int ledval=1; //LED出力値 @@ -40,6 +42,14 @@ int ErrFlg=0; //エラー判定フラグ int SensValBuf=0; //センサ値のバッファ int SensorR,SensorL; //ラインセンサ +int SensR[9],SensL[9]; +double PGainCLB=0; //Pゲイン調整変数 +double IGainCLB=0; //Iゲイン調整変数 +double DGainCLB=1.7; //Dゲイン調整変数 +double SensVal_I,SensVal_IBuf=0; +char turnFlg=0; +char Stime; +int CommSpeed; //----------ボリューム値取得----------------- int GetVol(void){ int Val; @@ -48,9 +58,34 @@ } //----------センサ値更新----------------- void SensUp(void){ - SensorR = LineR.read_u16()>>8; - SensorL = LineL.read_u16()>>8; +int i,j,SensBuf; + for (i = 0; i < 9; i++) { + for (j = 9; j > i; j--) { + if (SensR[j-1] > SensR[j]) { + SensBuf = SensR[j-1]; + SensR[j-1] = SensR[j]; + SensR[j] = SensBuf; + } + if (SensL[j-1] > SensL[j]) { + SensBuf = SensL[j-1]; + SensL[j-1] = SensL[j]; + SensL[j] = SensBuf; + } } + } + + SensorR = SensR[5]; + SensorL = SensL[5]; + } +//----------センサー値の取得--------------- +void sensGet(){ + if(Stime >= 9) { + Stime=0; + } + SensR[Stime] = LineR.read_u16(); + SensL[Stime] = LineL.read_u16(); + Stime++; + } //------------モータ管理-------------------- void MotorCtrl(void){ int RMotorVal,LMotorVal; @@ -73,10 +108,11 @@ RMotorVal = MotorR; MotorR_Rev = FW;} - if(LMotorVal >= 1000) MotorL = 1000; - if(RMotorVal >= 1000) MotorR = 1000; + if(LMotorVal >= 100) LMotorVal = 100; + if(RMotorVal >= 100) RMotorVal = 100; PWM_L.pulsewidth_us(LMotorVal); //左PWM (0~1000) PWM_R.pulsewidth_us(RMotorVal); //右PWM (0~1000) + //ledval = int(RMotorVal*0.016); } L_Dir = MotorL_Rev; //右モータ回転方向(H:CW) @@ -84,11 +120,29 @@ } //------------ライントレース-------------------- void LineTrace(void){ - int SensVal,CommSpeed=100,PGain=1; - + int SensVal,SensVal_D; + double PGain=0.026,IGain=0.0002,DGain=0.7; + if(SensorR <= 5000 && SensorL <= 5000) CommSpeed=0; SensVal = SensorR - SensorL; - MotorR = int(CommSpeed - (SensVal * PGain)); - MotorL = int(CommSpeed + (SensVal * PGain)); + if(SensVal <= 0 && !turnFlg){//turnL + turnFlg=1; + SensVal_I = 0; + } + else if (SensVal >= 0 && turnFlg){//turnR + turnFlg=0; + SensVal_I = 0; + } + SensVal_I = SensVal_I + SensVal; + SensVal_IBuf = SensVal_IBuf + SensVal; + if(SensVal_I >= 100000000) SensVal_I = 100000000; + if(SensVal_I <= (-100000000)) SensVal_I = (-100000000); + SensVal_D = SensValBuf - SensVal; + SensValBuf = SensVal; + MotorR = int((CommSpeed - ((SensVal * PGain)+(SensVal_I*IGain)-(SensVal_D*DGainCLB))))/10; + MotorL = int(CommSpeed + ((SensVal * PGain)+(SensVal_I*IGain)-(SensVal_D*DGainCLB)))/10; + ledval = int(SensVal_IBuf / (-100000)); + //MotorR = int(CommSpeed - (SensVal * PGainCLB)); + //MotorL = int(CommSpeed + (SensVal * PGainCLB)); } //-------------LED出力------------------ void led_out(void){ @@ -104,11 +158,11 @@ //----------タイマー割り込み--------------- void flip(){ - timer1++; MotorCtrl(); led_out(); SensUp(); LineTrace(); + timer1++; } //----------マイコン初期設定--------------- @@ -124,22 +178,37 @@ //割り込み処理開始 flipper.attach_us(&flip,1000); //汎用タイマー割り込み +sensget.attach_us(&sensGet,100); //センサー用タイマー割り込み //PWM周期設定 -PWM_L.period(0.001); -PWM_R.period(0.001); +PWM_L.period(0.0001); +PWM_R.period(0.0001); } //---------------メイン-------------------- int main() { init(); int sensV; + wait(1); - while(SW_IN); + while(SW_IN) + { + pc.printf("%5d %5d \r\n",SensorL,SensorR); + wait(0.1); + } + wait(1); MotorDA = 0; + timer1 = 0; while(1) { - if(!SW_IN) pc.printf("%5d %5d %5d %5d \r\n",SensorL,SensorR,MotorL,MotorR); - ledval = sensV; - wait(0.02); + if(!SW_IN) + { + DGainCLB = DGainCLB + 0.1; + pc.printf("%5d %5d %5d %5d %f %d \r\n",SensorL,SensorR,MotorL,MotorR,DGainCLB,turnFlg); + //ledval++; + } + if(timer1 <= 1000)SensVal_IBuf = 0; + if(SensVal_I <= (-10000) || SensVal_I >= 10000 )CommSpeed = 200; + else CommSpeed=490; + wait(0.01); } } \ No newline at end of file