Takeshi Nishimura / Mbed 2 deprecated MiniTracer2020_V1

sample

Dependencies:   mbed

https://os.mbed.com/media/uploads/MCR_Xavier/x-board.pdf

main.cpp@1:f382b4244a89, 2020-11-08 (annotated)

Committer:
MCR_Xavier
Date:
Sun Nov 08 03:59:27 2020 +0000
Revision:
1:f382b4244a89
Parent:
0:21afd0549d07 
sample

Who changed what in which revision?

UserRevisionLine numberNew 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 1:f382b4244a89 43 float VolumeVal;
MCR_Xavier 0:21afd0549d07 44 //----------ボリューム値取得-----------------
MCR_Xavier 0:21afd0549d07 45 int GetVol(void){
MCR_Xavier 0:21afd0549d07 46 int Val;
MCR_Xavier 1:f382b4244a89 47 Val = Volume.read_u16()>>7;
MCR_Xavier 0:21afd0549d07 48 return Val;
MCR_Xavier 0:21afd0549d07 49 }
MCR_Xavier 0:21afd0549d07 50 //----------センサ値更新-----------------
MCR_Xavier 0:21afd0549d07 51 void SensUp(void){
MCR_Xavier 1:f382b4244a89 52 SensorR = LineR.read_u16()>>4;
MCR_Xavier 1:f382b4244a89 53 SensorL = LineL.read_u16()>>4;
MCR_Xavier 0:21afd0549d07 54 }
MCR_Xavier 0:21afd0549d07 55 //------------モータ管理--------------------
MCR_Xavier 0:21afd0549d07 56 void MotorCtrl(void){
MCR_Xavier 0:21afd0549d07 57 int RMotorVal,LMotorVal;
MCR_Xavier 0:21afd0549d07 58 if(ErrFlg){ //異常判定
MCR_Xavier 0:21afd0549d07 59 PWM_L.pulsewidth_us(0); //左PWM出力0
MCR_Xavier 0:21afd0549d07 60 PWM_R.pulsewidth_us(0); //右PWM出力0
MCR_Xavier 0:21afd0549d07 61 }
MCR_Xavier 0:21afd0549d07 62 else{ //通常時
MCR_Xavier 0:21afd0549d07 63 if(MotorL < 0 ){
MCR_Xavier 0:21afd0549d07 64 LMotorVal = MotorL * (-1);
MCR_Xavier 0:21afd0549d07 65 MotorL_Rev = BW;}
MCR_Xavier 0:21afd0549d07 66 else{
MCR_Xavier 0:21afd0549d07 67 LMotorVal = MotorL;
MCR_Xavier 0:21afd0549d07 68 MotorL_Rev = FW;}
MCR_Xavier 0:21afd0549d07 69
MCR_Xavier 0:21afd0549d07 70 if(MotorR < 0 ){
MCR_Xavier 0:21afd0549d07 71 RMotorVal = MotorR * (-1);
MCR_Xavier 0:21afd0549d07 72 MotorR_Rev = BW;}
MCR_Xavier 0:21afd0549d07 73 else{
MCR_Xavier 0:21afd0549d07 74 RMotorVal = MotorR;
MCR_Xavier 0:21afd0549d07 75 MotorR_Rev = FW;}
MCR_Xavier 0:21afd0549d07 76
MCR_Xavier 0:21afd0549d07 77 if(LMotorVal >= 1000) MotorL = 1000;
MCR_Xavier 0:21afd0549d07 78 if(RMotorVal >= 1000) MotorR = 1000;
MCR_Xavier 0:21afd0549d07 79 PWM_L.pulsewidth_us(LMotorVal); //左PWM (0~1000)
MCR_Xavier 0:21afd0549d07 80 PWM_R.pulsewidth_us(RMotorVal); //右PWM (0~1000)
MCR_Xavier 0:21afd0549d07 81 }
MCR_Xavier 0:21afd0549d07 82
MCR_Xavier 0:21afd0549d07 83 L_Dir = MotorL_Rev; //右モータ回転方向（H：CW）
MCR_Xavier 0:21afd0549d07 84 R_Dir = !MotorR_Rev; //左モータ回転方向（L：FW）
MCR_Xavier 0:21afd0549d07 85 }
MCR_Xavier 0:21afd0549d07 86 //------------ライントレース--------------------
MCR_Xavier 0:21afd0549d07 87 void LineTrace(void){
MCR_Xavier 1:f382b4244a89 88 long SensVal,CommSpeed=0,sensdelta=0,SensValBuf=0,SensI;
MCR_Xavier 1:f382b4244a89 89 //float PGain=6.3,DGain=4;
MCR_Xavier 1:f382b4244a89 90 //float PGain=0.18,DGain=1.07;
MCR_Xavier 1:f382b4244a89 91 float PGain=0.75,DGain=0,IGain=0.1;
MCR_Xavier 1:f382b4244a89 92 //DGain=0;
MCR_Xavier 1:f382b4244a89 93 DGain=VolumeVal;
MCR_Xavier 1:f382b4244a89 94 //PGain=VolumeVal;
MCR_Xavier 1:f382b4244a89 95 //IGain=VolumeVal;
MCR_Xavier 1:f382b4244a89 96 //CommSpeed=100*VolumeVal;
MCR_Xavier 0:21afd0549d07 97 SensVal = SensorR - SensorL;
MCR_Xavier 1:f382b4244a89 98 sensdelta = SensVal - SensValBuf;
MCR_Xavier 1:f382b4244a89 99 SensI = SensI + SensVal;
MCR_Xavier 1:f382b4244a89 100 if(sensdelta < 0 ) sensdelta = sensdelta * (-1);
MCR_Xavier 1:f382b4244a89 101 MotorR = int(CommSpeed - (SensVal * PGain) - (sensdelta * DGain) - (SensI * IGain));
MCR_Xavier 1:f382b4244a89 102 //MotorL = int(1.19 * (CommSpeed + (SensVal * PGain) - (sensdelta * DGain)));
MCR_Xavier 1:f382b4244a89 103 MotorL = int(CommSpeed + (SensVal * PGain) - (sensdelta * DGain) + (SensI * IGain));
MCR_Xavier 1:f382b4244a89 104 SensValBuf = SensVal;
MCR_Xavier 0:21afd0549d07 105 }
MCR_Xavier 0:21afd0549d07 106 //-------------LED出力------------------
MCR_Xavier 0:21afd0549d07 107 void led_out(void){
MCR_Xavier 0:21afd0549d07 108 if(ledval & 0x01) led1 = 1;
MCR_Xavier 0:21afd0549d07 109 else led1 = 0;
MCR_Xavier 0:21afd0549d07 110 if(ledval & 0x02) led2 = 1;
MCR_Xavier 0:21afd0549d07 111 else led2 = 0;
MCR_Xavier 0:21afd0549d07 112 if(ledval & 0x04) led3 = 1;
MCR_Xavier 0:21afd0549d07 113 else led3 = 0;
MCR_Xavier 0:21afd0549d07 114 if(ledval & 0x08) led4 = 1;
MCR_Xavier 0:21afd0549d07 115 else led4 = 0;
MCR_Xavier 0:21afd0549d07 116 }
MCR_Xavier 0:21afd0549d07 117
MCR_Xavier 0:21afd0549d07 118 //----------タイマー割り込み---------------
MCR_Xavier 0:21afd0549d07 119 void flip(){
MCR_Xavier 0:21afd0549d07 120 timer1++;
MCR_Xavier 0:21afd0549d07 121 MotorCtrl();
MCR_Xavier 0:21afd0549d07 122 led_out();
MCR_Xavier 0:21afd0549d07 123 SensUp();
MCR_Xavier 1:f382b4244a89 124 //LineTrace();
MCR_Xavier 0:21afd0549d07 125 }
MCR_Xavier 0:21afd0549d07 126
MCR_Xavier 0:21afd0549d07 127 //----------マイコン初期設定---------------
MCR_Xavier 0:21afd0549d07 128 void init(void){
MCR_Xavier 0:21afd0549d07 129 //I/O設定
MCR_Xavier 0:21afd0549d07 130 /*
MCR_Xavier 0:21afd0549d07 131 RS1.mode(PullDown); //ロータリスイッチbit1
MCR_Xavier 0:21afd0549d07 132 RS2.mode(PullDown); //ロータリスイッチbit2
MCR_Xavier 0:21afd0549d07 133 RS3.mode(PullDown); //ロータリスイッチbit3
MCR_Xavier 0:21afd0549d07 134 RS4.mode(PullDown); //ロータリスイッチbit4*/
MCR_Xavier 0:21afd0549d07 135 SW_IN.mode(PullUp); //スイッチ入力ピンプルアップ
MCR_Xavier 0:21afd0549d07 136 MotorDA = 1;
MCR_Xavier 0:21afd0549d07 137
MCR_Xavier 0:21afd0549d07 138 //割り込み処理開始
MCR_Xavier 0:21afd0549d07 139 flipper.attach_us(&flip,1000); //汎用タイマー割り込み
MCR_Xavier 0:21afd0549d07 140 //PWM周期設定
MCR_Xavier 0:21afd0549d07 141 PWM_L.period(0.001);
MCR_Xavier 0:21afd0549d07 142 PWM_R.period(0.001);
MCR_Xavier 0:21afd0549d07 143 }
MCR_Xavier 0:21afd0549d07 144
MCR_Xavier 0:21afd0549d07 145 //---------------メイン--------------------
MCR_Xavier 0:21afd0549d07 146 int main() {
MCR_Xavier 0:21afd0549d07 147 init();
MCR_Xavier 1:f382b4244a89 148 float tmpval;
MCR_Xavier 0:21afd0549d07 149 wait(1);
MCR_Xavier 0:21afd0549d07 150 while(SW_IN);
MCR_Xavier 0:21afd0549d07 151 MotorDA = 0;
MCR_Xavier 0:21afd0549d07 152 while(1) {
MCR_Xavier 1:f382b4244a89 153 //tmpval=GetVol();
MCR_Xavier 1:f382b4244a89 154 //VolumeVal=tmpval/100;
MCR_Xavier 1:f382b4244a89 155 if(!SW_IN) pc.printf("%5d %5d %5d %5d %f \r\n",SensorL,SensorR,MotorL,MotorR,VolumeVal);
MCR_Xavier 1:f382b4244a89 156 //ledval = sensV;
MCR_Xavier 1:f382b4244a89 157 MotorR = 300;
MCR_Xavier 1:f382b4244a89 158 MotorL = 300;
MCR_Xavier 1:f382b4244a89 159 ledval ++;
MCR_Xavier 1:f382b4244a89 160 wait(0.5);
MCR_Xavier 1:f382b4244a89 161 MotorR = -300;
MCR_Xavier 1:f382b4244a89 162 MotorL = -300;
MCR_Xavier 1:f382b4244a89 163 ledval ++;
MCR_Xavier 1:f382b4244a89 164 wait(0.5);
MCR_Xavier 0:21afd0549d07 165
MCR_Xavier 0:21afd0549d07 166 }
MCR_Xavier 0:21afd0549d07 167 }