Takeshi Nishimura
/
MiniTracer2020_V1
sample
https://os.mbed.com/media/uploads/MCR_Xavier/x-board.pdf
main.cpp
- Committer:
- MCR_Xavier
- Date:
- 2020-11-08
- Revision:
- 1:f382b4244a89
- Parent:
- 0:21afd0549d07
File content as of revision 1:f382b4244a89:
#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; //汎用タイマー //プロトタイプ宣言 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; //ラインセンサ float VolumeVal; //----------ボリューム値取得----------------- int GetVol(void){ int Val; Val = Volume.read_u16()>>7; return Val; } //----------センサ値更新----------------- void SensUp(void){ SensorR = LineR.read_u16()>>4; SensorL = LineL.read_u16()>>4; } //------------モータ管理-------------------- 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 >= 1000) MotorL = 1000; if(RMotorVal >= 1000) MotorR = 1000; PWM_L.pulsewidth_us(LMotorVal); //左PWM (0~1000) PWM_R.pulsewidth_us(RMotorVal); //右PWM (0~1000) } L_Dir = MotorL_Rev; //右モータ回転方向(H:CW) R_Dir = !MotorR_Rev; //左モータ回転方向(L:FW) } //------------ライントレース-------------------- void LineTrace(void){ long SensVal,CommSpeed=0,sensdelta=0,SensValBuf=0,SensI; //float PGain=6.3,DGain=4; //float PGain=0.18,DGain=1.07; float PGain=0.75,DGain=0,IGain=0.1; //DGain=0; DGain=VolumeVal; //PGain=VolumeVal; //IGain=VolumeVal; //CommSpeed=100*VolumeVal; SensVal = SensorR - SensorL; sensdelta = SensVal - SensValBuf; SensI = SensI + SensVal; if(sensdelta < 0 ) sensdelta = sensdelta * (-1); MotorR = int(CommSpeed - (SensVal * PGain) - (sensdelta * DGain) - (SensI * IGain)); //MotorL = int(1.19 * (CommSpeed + (SensVal * PGain) - (sensdelta * DGain))); MotorL = int(CommSpeed + (SensVal * PGain) - (sensdelta * DGain) + (SensI * IGain)); SensValBuf = SensVal; } //-------------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(){ timer1++; MotorCtrl(); led_out(); SensUp(); //LineTrace(); } //----------マイコン初期設定--------------- 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); //汎用タイマー割り込み //PWM周期設定 PWM_L.period(0.001); PWM_R.period(0.001); } //---------------メイン-------------------- int main() { init(); float tmpval; wait(1); while(SW_IN); MotorDA = 0; while(1) { //tmpval=GetVol(); //VolumeVal=tmpval/100; if(!SW_IN) pc.printf("%5d %5d %5d %5d %f \r\n",SensorL,SensorR,MotorL,MotorR,VolumeVal); //ledval = sensV; MotorR = 300; MotorL = 300; ledval ++; wait(0.5); MotorR = -300; MotorL = -300; ledval ++; wait(0.5); } }