Takeshi Nishimura
/
MiniTracer2021
2021sample
main.cpp
- Committer:
- MCR_Xavier
- Date:
- 2019-06-09
- Revision:
- 1:c5854b9559f9
- Parent:
- 0:21afd0549d07
- Child:
- 2:db9d6e8ac3c1
File content as of revision 1:c5854b9559f9:
#include "mbed.h" //シンボル定義 #define FW 0 //前進 #define BW 1 //後進 AnalogIn LineL(PA_0); //アナログラインセンサ AnalogIn LineR(PA_1); //アナログラインセンサ AnalogIn Volume(PA_4); //ボリューム入力 DigitalOut led1(PB_7); //LED_1 DigitalOut led2(PB_6); //LED_2 DigitalOut led3(PB_5); //LED_3 DigitalOut led4(PB_4); //LED_4 DigitalOut L_Dir(PA_12); //左モータ回転方向 DigitalOut R_Dir(PA_9); //右モータ回転方向 DigitalOut MotorDA(PA_11); //モーター出力EN DigitalIn SW_IN(PB_3); //スイッチ入力 PwmOut PWM_L(PA_8); //左モータPWM PwmOut PWM_R(PA_10); //右モータPWM //AQM0802A lcd(PB_7,PB_6); Serial pc(USBTX, USBRX); // tx, rx //割り込み定義 Ticker flipper; //汎用タイマー Ticker sensget; //センサー用タイマー //プロトタイプ宣言 void init(void); //マイコン初期設定 void led_out(void); //LED出力 void MotorCtrl(void); //モータ管理 void LineTrace(void); //ライントレース void SensUp(void); //センサー値更新 int GetVol(void); //ボリューム値取得 //グローバル変数の宣言 int timer1=0; //汎用タイマー int ledval=1; //LED出力値 int MotorL_Rev=0,MotorR_Rev=0; //モータの回転方向 int MotorL=0,MotorR=0; //モータPWMデューティ比 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; Val = Volume.read_u16()>>12; return Val; } //----------センサ値更新----------------- void SensUp(void){ 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; if(ErrFlg){ //異常判定 PWM_L.pulsewidth_us(0); //左PWM出力0 PWM_R.pulsewidth_us(0); //右PWM出力0 } else{ //通常時 if(MotorL < 0 ){ LMotorVal = MotorL * (-1); MotorL_Rev = BW;} else{ LMotorVal = MotorL; MotorL_Rev = FW;} if(MotorR < 0 ){ RMotorVal = MotorR * (-1); MotorR_Rev = BW;} else{ RMotorVal = MotorR; MotorR_Rev = FW;} 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) R_Dir = !MotorR_Rev; //左モータ回転方向(L:FW) } //------------ライントレース-------------------- void LineTrace(void){ int SensVal,SensVal_D; double PGain=0.026,IGain=0.0002,DGain=0.7; if(SensorR <= 5000 && SensorL <= 5000) CommSpeed=0; SensVal = SensorR - SensorL; 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){ if(ledval & 0x01) led1 = 1; else led1 = 0; if(ledval & 0x02) led2 = 1; else led2 = 0; if(ledval & 0x04) led3 = 1; else led3 = 0; if(ledval & 0x08) led4 = 1; else led4 = 0; } //----------タイマー割り込み--------------- void flip(){ MotorCtrl(); led_out(); SensUp(); LineTrace(); timer1++; } //----------マイコン初期設定--------------- void init(void){ //I/O設定 /* RS1.mode(PullDown); //ロータリスイッチbit1 RS2.mode(PullDown); //ロータリスイッチbit2 RS3.mode(PullDown); //ロータリスイッチbit3 RS4.mode(PullDown); //ロータリスイッチbit4*/ SW_IN.mode(PullUp); //スイッチ入力ピンプルアップ MotorDA = 1; //割り込み処理開始 flipper.attach_us(&flip,1000); //汎用タイマー割り込み sensget.attach_us(&sensGet,100); //センサー用タイマー割り込み //PWM周期設定 PWM_L.period(0.0001); PWM_R.period(0.0001); } //---------------メイン-------------------- int main() { init(); int sensV; wait(1); 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) { 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); } }