Takeshi Nishimura
/
MiniTracer2020_V1
sample
https://os.mbed.com/media/uploads/MCR_Xavier/x-board.pdf
Diff: main.cpp
- Revision:
- 0:21afd0549d07
- Child:
- 1:f382b4244a89
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Sat Apr 28 06:26:41 2018 +0000 @@ -0,0 +1,145 @@ +#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); + + } +} \ No newline at end of file