Takeshi Nishimura / Mbed 2 deprecated MiniTracer2021

2021sample

Dependencies:   mbed

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?

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 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 }