Takeshi Nishimura
/
MiniTracer2020_V1
sample
https://os.mbed.com/media/uploads/MCR_Xavier/x-board.pdf
main.cpp
- Committer:
- MCR_Xavier
- Date:
- 2018-04-28
- Revision:
- 0:21afd0549d07
- Child:
- 1:f382b4244a89
File content as of revision 0:21afd0549d07:
#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; //ラインセンサ //----------ボリューム値取得----------------- int GetVol(void){ int Val; Val = Volume.read_u16()>>12; return Val; } //----------センサ値更新----------------- void SensUp(void){ SensorR = LineR.read_u16()>>8; SensorL = LineL.read_u16()>>8; } //------------モータ管理-------------------- 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){ int SensVal,CommSpeed=100,PGain=1; SensVal = SensorR - SensorL; MotorR = int(CommSpeed - (SensVal * PGain)); MotorL = int(CommSpeed + (SensVal * PGain)); } //-------------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(); int sensV; wait(1); while(SW_IN); MotorDA = 0; while(1) { if(!SW_IN) pc.printf("%5d %5d %5d %5d \r\n",SensorL,SensorR,MotorL,MotorR); ledval = sensV; wait(0.02); } }