Takeshi Nishimura
/
MiniTracer2021
2021sample
main.cpp@2:db9d6e8ac3c1, 2021-05-29 (annotated)
- Committer:
- MCR_Xavier
- Date:
- Sat May 29 04:58:51 2021 +0000
- Revision:
- 2:db9d6e8ac3c1
- Parent:
- 1:c5854b9559f9
Minitrace2021sample
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 | 1:c5854b9559f9 | 26 | Ticker sensget; //センサー用タイマー |
MCR_Xavier | 0:21afd0549d07 | 27 | |
MCR_Xavier | 0:21afd0549d07 | 28 | //プロトタイプ宣言 |
MCR_Xavier | 0:21afd0549d07 | 29 | void init(void); //マイコン初期設定 |
MCR_Xavier | 0:21afd0549d07 | 30 | void led_out(void); //LED出力 |
MCR_Xavier | 0:21afd0549d07 | 31 | void MotorCtrl(void); //モータ管理 |
MCR_Xavier | 0:21afd0549d07 | 32 | void LineTrace(void); //ライントレース |
MCR_Xavier | 0:21afd0549d07 | 33 | void SensUp(void); //センサー値更新 |
MCR_Xavier | 0:21afd0549d07 | 34 | int GetVol(void); //ボリューム値取得 |
MCR_Xavier | 0:21afd0549d07 | 35 | |
MCR_Xavier | 1:c5854b9559f9 | 36 | |
MCR_Xavier | 0:21afd0549d07 | 37 | //グローバル変数の宣言 |
MCR_Xavier | 0:21afd0549d07 | 38 | int timer1=0; //汎用タイマー |
MCR_Xavier | 0:21afd0549d07 | 39 | int ledval=1; //LED出力値 |
MCR_Xavier | 0:21afd0549d07 | 40 | int MotorL_Rev=0,MotorR_Rev=0; //モータの回転方向 |
MCR_Xavier | 0:21afd0549d07 | 41 | int MotorL=0,MotorR=0; //モータPWMデューティ比 |
MCR_Xavier | 0:21afd0549d07 | 42 | int ErrFlg=0; //エラー判定フラグ |
MCR_Xavier | 0:21afd0549d07 | 43 | int SensValBuf=0; //センサ値のバッファ |
MCR_Xavier | 0:21afd0549d07 | 44 | int SensorR,SensorL; //ラインセンサ |
MCR_Xavier | 1:c5854b9559f9 | 45 | int SensR[9],SensL[9]; |
MCR_Xavier | 1:c5854b9559f9 | 46 | double PGainCLB=0; //Pゲイン調整変数 |
MCR_Xavier | 1:c5854b9559f9 | 47 | double IGainCLB=0; //Iゲイン調整変数 |
MCR_Xavier | 2:db9d6e8ac3c1 | 48 | double DGainCLB=0;//1.7; //Dゲイン調整変数 |
MCR_Xavier | 1:c5854b9559f9 | 49 | double SensVal_I,SensVal_IBuf=0; |
MCR_Xavier | 1:c5854b9559f9 | 50 | char turnFlg=0; |
MCR_Xavier | 1:c5854b9559f9 | 51 | char Stime; |
MCR_Xavier | 1:c5854b9559f9 | 52 | int CommSpeed; |
MCR_Xavier | 0:21afd0549d07 | 53 | //----------ボリューム値取得----------------- |
MCR_Xavier | 0:21afd0549d07 | 54 | int GetVol(void){ |
MCR_Xavier | 0:21afd0549d07 | 55 | int Val; |
MCR_Xavier | 0:21afd0549d07 | 56 | Val = Volume.read_u16()>>12; |
MCR_Xavier | 0:21afd0549d07 | 57 | return Val; |
MCR_Xavier | 0:21afd0549d07 | 58 | } |
MCR_Xavier | 0:21afd0549d07 | 59 | //----------センサ値更新----------------- |
MCR_Xavier | 0:21afd0549d07 | 60 | void SensUp(void){ |
MCR_Xavier | 1:c5854b9559f9 | 61 | int i,j,SensBuf; |
MCR_Xavier | 1:c5854b9559f9 | 62 | for (i = 0; i < 9; i++) { |
MCR_Xavier | 1:c5854b9559f9 | 63 | for (j = 9; j > i; j--) { |
MCR_Xavier | 1:c5854b9559f9 | 64 | if (SensR[j-1] > SensR[j]) { |
MCR_Xavier | 1:c5854b9559f9 | 65 | SensBuf = SensR[j-1]; |
MCR_Xavier | 1:c5854b9559f9 | 66 | SensR[j-1] = SensR[j]; |
MCR_Xavier | 1:c5854b9559f9 | 67 | SensR[j] = SensBuf; |
MCR_Xavier | 1:c5854b9559f9 | 68 | } |
MCR_Xavier | 1:c5854b9559f9 | 69 | if (SensL[j-1] > SensL[j]) { |
MCR_Xavier | 1:c5854b9559f9 | 70 | SensBuf = SensL[j-1]; |
MCR_Xavier | 1:c5854b9559f9 | 71 | SensL[j-1] = SensL[j]; |
MCR_Xavier | 1:c5854b9559f9 | 72 | SensL[j] = SensBuf; |
MCR_Xavier | 1:c5854b9559f9 | 73 | } |
MCR_Xavier | 0:21afd0549d07 | 74 | } |
MCR_Xavier | 1:c5854b9559f9 | 75 | } |
MCR_Xavier | 1:c5854b9559f9 | 76 | |
MCR_Xavier | 1:c5854b9559f9 | 77 | SensorR = SensR[5]; |
MCR_Xavier | 1:c5854b9559f9 | 78 | SensorL = SensL[5]; |
MCR_Xavier | 1:c5854b9559f9 | 79 | } |
MCR_Xavier | 1:c5854b9559f9 | 80 | //----------センサー値の取得--------------- |
MCR_Xavier | 1:c5854b9559f9 | 81 | void sensGet(){ |
MCR_Xavier | 1:c5854b9559f9 | 82 | if(Stime >= 9) { |
MCR_Xavier | 1:c5854b9559f9 | 83 | Stime=0; |
MCR_Xavier | 1:c5854b9559f9 | 84 | } |
MCR_Xavier | 1:c5854b9559f9 | 85 | SensR[Stime] = LineR.read_u16(); |
MCR_Xavier | 1:c5854b9559f9 | 86 | SensL[Stime] = LineL.read_u16(); |
MCR_Xavier | 1:c5854b9559f9 | 87 | Stime++; |
MCR_Xavier | 1:c5854b9559f9 | 88 | } |
MCR_Xavier | 0:21afd0549d07 | 89 | //------------モータ管理-------------------- |
MCR_Xavier | 0:21afd0549d07 | 90 | void MotorCtrl(void){ |
MCR_Xavier | 0:21afd0549d07 | 91 | int RMotorVal,LMotorVal; |
MCR_Xavier | 0:21afd0549d07 | 92 | if(ErrFlg){ //異常判定 |
MCR_Xavier | 0:21afd0549d07 | 93 | PWM_L.pulsewidth_us(0); //左PWM出力0 |
MCR_Xavier | 0:21afd0549d07 | 94 | PWM_R.pulsewidth_us(0); //右PWM出力0 |
MCR_Xavier | 0:21afd0549d07 | 95 | } |
MCR_Xavier | 0:21afd0549d07 | 96 | else{ //通常時 |
MCR_Xavier | 0:21afd0549d07 | 97 | if(MotorL < 0 ){ |
MCR_Xavier | 0:21afd0549d07 | 98 | LMotorVal = MotorL * (-1); |
MCR_Xavier | 0:21afd0549d07 | 99 | MotorL_Rev = BW;} |
MCR_Xavier | 0:21afd0549d07 | 100 | else{ |
MCR_Xavier | 0:21afd0549d07 | 101 | LMotorVal = MotorL; |
MCR_Xavier | 0:21afd0549d07 | 102 | MotorL_Rev = FW;} |
MCR_Xavier | 0:21afd0549d07 | 103 | |
MCR_Xavier | 0:21afd0549d07 | 104 | if(MotorR < 0 ){ |
MCR_Xavier | 0:21afd0549d07 | 105 | RMotorVal = MotorR * (-1); |
MCR_Xavier | 0:21afd0549d07 | 106 | MotorR_Rev = BW;} |
MCR_Xavier | 0:21afd0549d07 | 107 | else{ |
MCR_Xavier | 0:21afd0549d07 | 108 | RMotorVal = MotorR; |
MCR_Xavier | 0:21afd0549d07 | 109 | MotorR_Rev = FW;} |
MCR_Xavier | 0:21afd0549d07 | 110 | |
MCR_Xavier | 1:c5854b9559f9 | 111 | if(LMotorVal >= 100) LMotorVal = 100; |
MCR_Xavier | 1:c5854b9559f9 | 112 | if(RMotorVal >= 100) RMotorVal = 100; |
MCR_Xavier | 0:21afd0549d07 | 113 | PWM_L.pulsewidth_us(LMotorVal); //左PWM (0~1000) |
MCR_Xavier | 0:21afd0549d07 | 114 | PWM_R.pulsewidth_us(RMotorVal); //右PWM (0~1000) |
MCR_Xavier | 1:c5854b9559f9 | 115 | //ledval = int(RMotorVal*0.016); |
MCR_Xavier | 0:21afd0549d07 | 116 | } |
MCR_Xavier | 0:21afd0549d07 | 117 | |
MCR_Xavier | 2:db9d6e8ac3c1 | 118 | L_Dir = !MotorL_Rev; //右モータ回転方向(H:CW) |
MCR_Xavier | 0:21afd0549d07 | 119 | R_Dir = !MotorR_Rev; //左モータ回転方向(L:FW) |
MCR_Xavier | 0:21afd0549d07 | 120 | } |
MCR_Xavier | 0:21afd0549d07 | 121 | //------------ライントレース-------------------- |
MCR_Xavier | 0:21afd0549d07 | 122 | void LineTrace(void){ |
MCR_Xavier | 1:c5854b9559f9 | 123 | int SensVal,SensVal_D; |
MCR_Xavier | 2:db9d6e8ac3c1 | 124 | double PGain=0.026,IGain=0.0002,DGain=1.6; |
MCR_Xavier | 1:c5854b9559f9 | 125 | if(SensorR <= 5000 && SensorL <= 5000) CommSpeed=0; |
MCR_Xavier | 0:21afd0549d07 | 126 | SensVal = SensorR - SensorL; |
MCR_Xavier | 1:c5854b9559f9 | 127 | if(SensVal <= 0 && !turnFlg){//turnL |
MCR_Xavier | 1:c5854b9559f9 | 128 | turnFlg=1; |
MCR_Xavier | 1:c5854b9559f9 | 129 | SensVal_I = 0; |
MCR_Xavier | 1:c5854b9559f9 | 130 | } |
MCR_Xavier | 1:c5854b9559f9 | 131 | else if (SensVal >= 0 && turnFlg){//turnR |
MCR_Xavier | 1:c5854b9559f9 | 132 | turnFlg=0; |
MCR_Xavier | 1:c5854b9559f9 | 133 | SensVal_I = 0; |
MCR_Xavier | 1:c5854b9559f9 | 134 | } |
MCR_Xavier | 1:c5854b9559f9 | 135 | SensVal_I = SensVal_I + SensVal; |
MCR_Xavier | 1:c5854b9559f9 | 136 | SensVal_IBuf = SensVal_IBuf + SensVal; |
MCR_Xavier | 1:c5854b9559f9 | 137 | if(SensVal_I >= 100000000) SensVal_I = 100000000; |
MCR_Xavier | 1:c5854b9559f9 | 138 | if(SensVal_I <= (-100000000)) SensVal_I = (-100000000); |
MCR_Xavier | 1:c5854b9559f9 | 139 | SensVal_D = SensValBuf - SensVal; |
MCR_Xavier | 1:c5854b9559f9 | 140 | SensValBuf = SensVal; |
MCR_Xavier | 2:db9d6e8ac3c1 | 141 | MotorR = int(((CommSpeed - ((SensVal * PGain)+(SensVal_I*IGain)-(SensVal_D*DGain))))/10); |
MCR_Xavier | 2:db9d6e8ac3c1 | 142 | MotorL = int((CommSpeed + ((SensVal * PGain)+(SensVal_I*IGain)-(SensVal_D*DGain)))/10); |
MCR_Xavier | 1:c5854b9559f9 | 143 | ledval = int(SensVal_IBuf / (-100000)); |
MCR_Xavier | 1:c5854b9559f9 | 144 | //MotorR = int(CommSpeed - (SensVal * PGainCLB)); |
MCR_Xavier | 1:c5854b9559f9 | 145 | //MotorL = int(CommSpeed + (SensVal * PGainCLB)); |
MCR_Xavier | 0:21afd0549d07 | 146 | } |
MCR_Xavier | 0:21afd0549d07 | 147 | //-------------LED出力------------------ |
MCR_Xavier | 0:21afd0549d07 | 148 | void led_out(void){ |
MCR_Xavier | 0:21afd0549d07 | 149 | if(ledval & 0x01) led1 = 1; |
MCR_Xavier | 0:21afd0549d07 | 150 | else led1 = 0; |
MCR_Xavier | 0:21afd0549d07 | 151 | if(ledval & 0x02) led2 = 1; |
MCR_Xavier | 0:21afd0549d07 | 152 | else led2 = 0; |
MCR_Xavier | 0:21afd0549d07 | 153 | if(ledval & 0x04) led3 = 1; |
MCR_Xavier | 0:21afd0549d07 | 154 | else led3 = 0; |
MCR_Xavier | 0:21afd0549d07 | 155 | if(ledval & 0x08) led4 = 1; |
MCR_Xavier | 0:21afd0549d07 | 156 | else led4 = 0; |
MCR_Xavier | 0:21afd0549d07 | 157 | } |
MCR_Xavier | 0:21afd0549d07 | 158 | |
MCR_Xavier | 0:21afd0549d07 | 159 | //----------タイマー割り込み--------------- |
MCR_Xavier | 0:21afd0549d07 | 160 | void flip(){ |
MCR_Xavier | 0:21afd0549d07 | 161 | MotorCtrl(); |
MCR_Xavier | 0:21afd0549d07 | 162 | led_out(); |
MCR_Xavier | 0:21afd0549d07 | 163 | SensUp(); |
MCR_Xavier | 0:21afd0549d07 | 164 | LineTrace(); |
MCR_Xavier | 1:c5854b9559f9 | 165 | timer1++; |
MCR_Xavier | 0:21afd0549d07 | 166 | } |
MCR_Xavier | 0:21afd0549d07 | 167 | |
MCR_Xavier | 0:21afd0549d07 | 168 | //----------マイコン初期設定--------------- |
MCR_Xavier | 0:21afd0549d07 | 169 | void init(void){ |
MCR_Xavier | 0:21afd0549d07 | 170 | //I/O設定 |
MCR_Xavier | 0:21afd0549d07 | 171 | /* |
MCR_Xavier | 0:21afd0549d07 | 172 | RS1.mode(PullDown); //ロータリスイッチbit1 |
MCR_Xavier | 0:21afd0549d07 | 173 | RS2.mode(PullDown); //ロータリスイッチbit2 |
MCR_Xavier | 0:21afd0549d07 | 174 | RS3.mode(PullDown); //ロータリスイッチbit3 |
MCR_Xavier | 0:21afd0549d07 | 175 | RS4.mode(PullDown); //ロータリスイッチbit4*/ |
MCR_Xavier | 0:21afd0549d07 | 176 | SW_IN.mode(PullUp); //スイッチ入力ピンプルアップ |
MCR_Xavier | 0:21afd0549d07 | 177 | MotorDA = 1; |
MCR_Xavier | 0:21afd0549d07 | 178 | |
MCR_Xavier | 0:21afd0549d07 | 179 | //割り込み処理開始 |
MCR_Xavier | 0:21afd0549d07 | 180 | flipper.attach_us(&flip,1000); //汎用タイマー割り込み |
MCR_Xavier | 1:c5854b9559f9 | 181 | sensget.attach_us(&sensGet,100); //センサー用タイマー割り込み |
MCR_Xavier | 0:21afd0549d07 | 182 | //PWM周期設定 |
MCR_Xavier | 1:c5854b9559f9 | 183 | PWM_L.period(0.0001); |
MCR_Xavier | 1:c5854b9559f9 | 184 | PWM_R.period(0.0001); |
MCR_Xavier | 0:21afd0549d07 | 185 | } |
MCR_Xavier | 0:21afd0549d07 | 186 | |
MCR_Xavier | 0:21afd0549d07 | 187 | //---------------メイン-------------------- |
MCR_Xavier | 0:21afd0549d07 | 188 | int main() { |
MCR_Xavier | 0:21afd0549d07 | 189 | init(); |
MCR_Xavier | 0:21afd0549d07 | 190 | int sensV; |
MCR_Xavier | 1:c5854b9559f9 | 191 | |
MCR_Xavier | 0:21afd0549d07 | 192 | wait(1); |
MCR_Xavier | 1:c5854b9559f9 | 193 | while(SW_IN) |
MCR_Xavier | 1:c5854b9559f9 | 194 | { |
MCR_Xavier | 1:c5854b9559f9 | 195 | pc.printf("%5d %5d \r\n",SensorL,SensorR); |
MCR_Xavier | 1:c5854b9559f9 | 196 | wait(0.1); |
MCR_Xavier | 1:c5854b9559f9 | 197 | } |
MCR_Xavier | 1:c5854b9559f9 | 198 | wait(1); |
MCR_Xavier | 0:21afd0549d07 | 199 | MotorDA = 0; |
MCR_Xavier | 1:c5854b9559f9 | 200 | timer1 = 0; |
MCR_Xavier | 0:21afd0549d07 | 201 | while(1) { |
MCR_Xavier | 1:c5854b9559f9 | 202 | if(!SW_IN) |
MCR_Xavier | 1:c5854b9559f9 | 203 | { |
MCR_Xavier | 2:db9d6e8ac3c1 | 204 | //DGainCLB = DGainCLB + 0.1; |
MCR_Xavier | 1:c5854b9559f9 | 205 | pc.printf("%5d %5d %5d %5d %f %d \r\n",SensorL,SensorR,MotorL,MotorR,DGainCLB,turnFlg); |
MCR_Xavier | 1:c5854b9559f9 | 206 | //ledval++; |
MCR_Xavier | 2:db9d6e8ac3c1 | 207 | wait(0.1); |
MCR_Xavier | 1:c5854b9559f9 | 208 | } |
MCR_Xavier | 1:c5854b9559f9 | 209 | if(timer1 <= 1000)SensVal_IBuf = 0; |
MCR_Xavier | 1:c5854b9559f9 | 210 | if(SensVal_I <= (-10000) || SensVal_I >= 10000 )CommSpeed = 200; |
MCR_Xavier | 2:db9d6e8ac3c1 | 211 | else CommSpeed=450; |
MCR_Xavier | 1:c5854b9559f9 | 212 | wait(0.01); |
MCR_Xavier | 0:21afd0549d07 | 213 | |
MCR_Xavier | 0:21afd0549d07 | 214 | } |
MCR_Xavier | 0:21afd0549d07 | 215 | } |