Takeshi Nishimura
/
MiniTracer2020_V1
sample
https://os.mbed.com/media/uploads/MCR_Xavier/x-board.pdf
main.cpp@0:21afd0549d07, 2018-04-28 (annotated)
- Committer:
- MCR_Xavier
- Date:
- Sat Apr 28 06:26:41 2018 +0000
- Revision:
- 0:21afd0549d07
- Child:
- 1:f382b4244a89
MiniTracer???????????
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
MCR_Xavier | 0:21afd0549d07 | 1 | #include "mbed.h" |
MCR_Xavier | 0:21afd0549d07 | 2 | |
MCR_Xavier | 0:21afd0549d07 | 3 | //シンボル定義 |
MCR_Xavier | 0:21afd0549d07 | 4 | #define FW 0 //前進 |
MCR_Xavier | 0:21afd0549d07 | 5 | #define BW 1 //後進 |
MCR_Xavier | 0:21afd0549d07 | 6 | |
MCR_Xavier | 0:21afd0549d07 | 7 | AnalogIn LineL(PA_0); //アナログラインセンサ |
MCR_Xavier | 0:21afd0549d07 | 8 | AnalogIn LineR(PA_1); //アナログラインセンサ |
MCR_Xavier | 0:21afd0549d07 | 9 | AnalogIn Volume(PA_4); //ボリューム入力 |
MCR_Xavier | 0:21afd0549d07 | 10 | DigitalOut led1(PB_7); //LED_1 |
MCR_Xavier | 0:21afd0549d07 | 11 | DigitalOut led2(PB_6); //LED_2 |
MCR_Xavier | 0:21afd0549d07 | 12 | DigitalOut led3(PB_5); //LED_3 |
MCR_Xavier | 0:21afd0549d07 | 13 | DigitalOut led4(PB_4); //LED_4 |
MCR_Xavier | 0:21afd0549d07 | 14 | DigitalOut L_Dir(PA_12); //左モータ回転方向 |
MCR_Xavier | 0:21afd0549d07 | 15 | DigitalOut R_Dir(PA_9); //右モータ回転方向 |
MCR_Xavier | 0:21afd0549d07 | 16 | DigitalOut MotorDA(PA_11); //モーター出力EN |
MCR_Xavier | 0:21afd0549d07 | 17 | DigitalIn SW_IN(PB_3); //スイッチ入力 |
MCR_Xavier | 0:21afd0549d07 | 18 | PwmOut PWM_L(PA_8); //左モータPWM |
MCR_Xavier | 0:21afd0549d07 | 19 | PwmOut PWM_R(PA_10); //右モータPWM |
MCR_Xavier | 0:21afd0549d07 | 20 | //AQM0802A lcd(PB_7,PB_6); |
MCR_Xavier | 0:21afd0549d07 | 21 | Serial pc(USBTX, USBRX); // tx, rx |
MCR_Xavier | 0:21afd0549d07 | 22 | |
MCR_Xavier | 0:21afd0549d07 | 23 | |
MCR_Xavier | 0:21afd0549d07 | 24 | //割り込み定義 |
MCR_Xavier | 0:21afd0549d07 | 25 | Ticker flipper; //汎用タイマー |
MCR_Xavier | 0:21afd0549d07 | 26 | |
MCR_Xavier | 0:21afd0549d07 | 27 | //プロトタイプ宣言 |
MCR_Xavier | 0:21afd0549d07 | 28 | void init(void); //マイコン初期設定 |
MCR_Xavier | 0:21afd0549d07 | 29 | void led_out(void); //LED出力 |
MCR_Xavier | 0:21afd0549d07 | 30 | void MotorCtrl(void); //モータ管理 |
MCR_Xavier | 0:21afd0549d07 | 31 | void LineTrace(void); //ライントレース |
MCR_Xavier | 0:21afd0549d07 | 32 | void SensUp(void); //センサー値更新 |
MCR_Xavier | 0:21afd0549d07 | 33 | int GetVol(void); //ボリューム値取得 |
MCR_Xavier | 0:21afd0549d07 | 34 | |
MCR_Xavier | 0:21afd0549d07 | 35 | //グローバル変数の宣言 |
MCR_Xavier | 0:21afd0549d07 | 36 | int timer1=0; //汎用タイマー |
MCR_Xavier | 0:21afd0549d07 | 37 | int ledval=1; //LED出力値 |
MCR_Xavier | 0:21afd0549d07 | 38 | int MotorL_Rev=0,MotorR_Rev=0; //モータの回転方向 |
MCR_Xavier | 0:21afd0549d07 | 39 | int MotorL=0,MotorR=0; //モータPWMデューティ比 |
MCR_Xavier | 0:21afd0549d07 | 40 | int ErrFlg=0; //エラー判定フラグ |
MCR_Xavier | 0:21afd0549d07 | 41 | int SensValBuf=0; //センサ値のバッファ |
MCR_Xavier | 0:21afd0549d07 | 42 | int SensorR,SensorL; //ラインセンサ |
MCR_Xavier | 0:21afd0549d07 | 43 | //----------ボリューム値取得----------------- |
MCR_Xavier | 0:21afd0549d07 | 44 | int GetVol(void){ |
MCR_Xavier | 0:21afd0549d07 | 45 | int Val; |
MCR_Xavier | 0:21afd0549d07 | 46 | Val = Volume.read_u16()>>12; |
MCR_Xavier | 0:21afd0549d07 | 47 | return Val; |
MCR_Xavier | 0:21afd0549d07 | 48 | } |
MCR_Xavier | 0:21afd0549d07 | 49 | //----------センサ値更新----------------- |
MCR_Xavier | 0:21afd0549d07 | 50 | void SensUp(void){ |
MCR_Xavier | 0:21afd0549d07 | 51 | SensorR = LineR.read_u16()>>8; |
MCR_Xavier | 0:21afd0549d07 | 52 | SensorL = LineL.read_u16()>>8; |
MCR_Xavier | 0:21afd0549d07 | 53 | } |
MCR_Xavier | 0:21afd0549d07 | 54 | //------------モータ管理-------------------- |
MCR_Xavier | 0:21afd0549d07 | 55 | void MotorCtrl(void){ |
MCR_Xavier | 0:21afd0549d07 | 56 | int RMotorVal,LMotorVal; |
MCR_Xavier | 0:21afd0549d07 | 57 | if(ErrFlg){ //異常判定 |
MCR_Xavier | 0:21afd0549d07 | 58 | PWM_L.pulsewidth_us(0); //左PWM出力0 |
MCR_Xavier | 0:21afd0549d07 | 59 | PWM_R.pulsewidth_us(0); //右PWM出力0 |
MCR_Xavier | 0:21afd0549d07 | 60 | } |
MCR_Xavier | 0:21afd0549d07 | 61 | else{ //通常時 |
MCR_Xavier | 0:21afd0549d07 | 62 | if(MotorL < 0 ){ |
MCR_Xavier | 0:21afd0549d07 | 63 | LMotorVal = MotorL * (-1); |
MCR_Xavier | 0:21afd0549d07 | 64 | MotorL_Rev = BW;} |
MCR_Xavier | 0:21afd0549d07 | 65 | else{ |
MCR_Xavier | 0:21afd0549d07 | 66 | LMotorVal = MotorL; |
MCR_Xavier | 0:21afd0549d07 | 67 | MotorL_Rev = FW;} |
MCR_Xavier | 0:21afd0549d07 | 68 | |
MCR_Xavier | 0:21afd0549d07 | 69 | if(MotorR < 0 ){ |
MCR_Xavier | 0:21afd0549d07 | 70 | RMotorVal = MotorR * (-1); |
MCR_Xavier | 0:21afd0549d07 | 71 | MotorR_Rev = BW;} |
MCR_Xavier | 0:21afd0549d07 | 72 | else{ |
MCR_Xavier | 0:21afd0549d07 | 73 | RMotorVal = MotorR; |
MCR_Xavier | 0:21afd0549d07 | 74 | MotorR_Rev = FW;} |
MCR_Xavier | 0:21afd0549d07 | 75 | |
MCR_Xavier | 0:21afd0549d07 | 76 | if(LMotorVal >= 1000) MotorL = 1000; |
MCR_Xavier | 0:21afd0549d07 | 77 | if(RMotorVal >= 1000) MotorR = 1000; |
MCR_Xavier | 0:21afd0549d07 | 78 | PWM_L.pulsewidth_us(LMotorVal); //左PWM (0~1000) |
MCR_Xavier | 0:21afd0549d07 | 79 | PWM_R.pulsewidth_us(RMotorVal); //右PWM (0~1000) |
MCR_Xavier | 0:21afd0549d07 | 80 | } |
MCR_Xavier | 0:21afd0549d07 | 81 | |
MCR_Xavier | 0:21afd0549d07 | 82 | L_Dir = MotorL_Rev; //右モータ回転方向(H:CW) |
MCR_Xavier | 0:21afd0549d07 | 83 | R_Dir = !MotorR_Rev; //左モータ回転方向(L:FW) |
MCR_Xavier | 0:21afd0549d07 | 84 | } |
MCR_Xavier | 0:21afd0549d07 | 85 | //------------ライントレース-------------------- |
MCR_Xavier | 0:21afd0549d07 | 86 | void LineTrace(void){ |
MCR_Xavier | 0:21afd0549d07 | 87 | int SensVal,CommSpeed=100,PGain=1; |
MCR_Xavier | 0:21afd0549d07 | 88 | |
MCR_Xavier | 0:21afd0549d07 | 89 | SensVal = SensorR - SensorL; |
MCR_Xavier | 0:21afd0549d07 | 90 | MotorR = int(CommSpeed - (SensVal * PGain)); |
MCR_Xavier | 0:21afd0549d07 | 91 | MotorL = int(CommSpeed + (SensVal * PGain)); |
MCR_Xavier | 0:21afd0549d07 | 92 | } |
MCR_Xavier | 0:21afd0549d07 | 93 | //-------------LED出力------------------ |
MCR_Xavier | 0:21afd0549d07 | 94 | void led_out(void){ |
MCR_Xavier | 0:21afd0549d07 | 95 | if(ledval & 0x01) led1 = 1; |
MCR_Xavier | 0:21afd0549d07 | 96 | else led1 = 0; |
MCR_Xavier | 0:21afd0549d07 | 97 | if(ledval & 0x02) led2 = 1; |
MCR_Xavier | 0:21afd0549d07 | 98 | else led2 = 0; |
MCR_Xavier | 0:21afd0549d07 | 99 | if(ledval & 0x04) led3 = 1; |
MCR_Xavier | 0:21afd0549d07 | 100 | else led3 = 0; |
MCR_Xavier | 0:21afd0549d07 | 101 | if(ledval & 0x08) led4 = 1; |
MCR_Xavier | 0:21afd0549d07 | 102 | else led4 = 0; |
MCR_Xavier | 0:21afd0549d07 | 103 | } |
MCR_Xavier | 0:21afd0549d07 | 104 | |
MCR_Xavier | 0:21afd0549d07 | 105 | //----------タイマー割り込み--------------- |
MCR_Xavier | 0:21afd0549d07 | 106 | void flip(){ |
MCR_Xavier | 0:21afd0549d07 | 107 | timer1++; |
MCR_Xavier | 0:21afd0549d07 | 108 | MotorCtrl(); |
MCR_Xavier | 0:21afd0549d07 | 109 | led_out(); |
MCR_Xavier | 0:21afd0549d07 | 110 | SensUp(); |
MCR_Xavier | 0:21afd0549d07 | 111 | LineTrace(); |
MCR_Xavier | 0:21afd0549d07 | 112 | } |
MCR_Xavier | 0:21afd0549d07 | 113 | |
MCR_Xavier | 0:21afd0549d07 | 114 | //----------マイコン初期設定--------------- |
MCR_Xavier | 0:21afd0549d07 | 115 | void init(void){ |
MCR_Xavier | 0:21afd0549d07 | 116 | //I/O設定 |
MCR_Xavier | 0:21afd0549d07 | 117 | /* |
MCR_Xavier | 0:21afd0549d07 | 118 | RS1.mode(PullDown); //ロータリスイッチbit1 |
MCR_Xavier | 0:21afd0549d07 | 119 | RS2.mode(PullDown); //ロータリスイッチbit2 |
MCR_Xavier | 0:21afd0549d07 | 120 | RS3.mode(PullDown); //ロータリスイッチbit3 |
MCR_Xavier | 0:21afd0549d07 | 121 | RS4.mode(PullDown); //ロータリスイッチbit4*/ |
MCR_Xavier | 0:21afd0549d07 | 122 | SW_IN.mode(PullUp); //スイッチ入力ピンプルアップ |
MCR_Xavier | 0:21afd0549d07 | 123 | MotorDA = 1; |
MCR_Xavier | 0:21afd0549d07 | 124 | |
MCR_Xavier | 0:21afd0549d07 | 125 | //割り込み処理開始 |
MCR_Xavier | 0:21afd0549d07 | 126 | flipper.attach_us(&flip,1000); //汎用タイマー割り込み |
MCR_Xavier | 0:21afd0549d07 | 127 | //PWM周期設定 |
MCR_Xavier | 0:21afd0549d07 | 128 | PWM_L.period(0.001); |
MCR_Xavier | 0:21afd0549d07 | 129 | PWM_R.period(0.001); |
MCR_Xavier | 0:21afd0549d07 | 130 | } |
MCR_Xavier | 0:21afd0549d07 | 131 | |
MCR_Xavier | 0:21afd0549d07 | 132 | //---------------メイン-------------------- |
MCR_Xavier | 0:21afd0549d07 | 133 | int main() { |
MCR_Xavier | 0:21afd0549d07 | 134 | init(); |
MCR_Xavier | 0:21afd0549d07 | 135 | int sensV; |
MCR_Xavier | 0:21afd0549d07 | 136 | wait(1); |
MCR_Xavier | 0:21afd0549d07 | 137 | while(SW_IN); |
MCR_Xavier | 0:21afd0549d07 | 138 | MotorDA = 0; |
MCR_Xavier | 0:21afd0549d07 | 139 | while(1) { |
MCR_Xavier | 0:21afd0549d07 | 140 | if(!SW_IN) pc.printf("%5d %5d %5d %5d \r\n",SensorL,SensorR,MotorL,MotorR); |
MCR_Xavier | 0:21afd0549d07 | 141 | ledval = sensV; |
MCR_Xavier | 0:21afd0549d07 | 142 | wait(0.02); |
MCR_Xavier | 0:21afd0549d07 | 143 | |
MCR_Xavier | 0:21afd0549d07 | 144 | } |
MCR_Xavier | 0:21afd0549d07 | 145 | } |