島根短大　ライントレーサ制作チーム / Mbed 2 deprecated line_trace_sample_array

配列を使用し記憶走行を行う

Dependencies:   mbed AQM0802 CRotaryEncoder TB6612FNG

main.cpp@5:f635f1f01d2d, 2019-08-26 (annotated)

Committer:
yusaku0125
Date:
Mon Aug 26 09:25:22 2019 +0000
Revision:
5:f635f1f01d2d
Parent:
4:ac9e6772ddb3
Child:
6:afd8f0d02c8d
test

Who changed what in which revision?

UserRevisionLine numberNew contents of line
yusaku0125 5:f635f1f01d2d 1 ////ライントレースサンプルver3
yusaku0125 5:f635f1f01d2d 2 ///モータ速度制御プログラムを追加。
yusaku0125 5:f635f1f01d2d 3 ///ラインセンサPD制御を追加。
yusaku0125 5:f635f1f01d2d 4 ///M_KP,M_KDは別途straight_speed_controlのプログラムで検証した値をセットする。
yusaku0125 5:f635f1f01d2d 5 ///フォトリフレクタのゲイン、ラインセンサの各種成分の調整を行い、
yusaku0125 5:f635f1f01d2d 6 ///ラインをきれいにトレースできるよう挑戦する。
yusaku0125 5:f635f1f01d2d 7
yusaku0125 3:e455433c8cae 8 #include "mbed.h"
yusaku0125 3:e455433c8cae 9 #include "CRotaryEncoder.h"
yusaku0125 4:ac9e6772ddb3 10 #include "TB6612.h"
yusaku0125 4:ac9e6772ddb3 11
yusaku0125 4:ac9e6772ddb3 12
yusaku0125 4:ac9e6772ddb3 13 //☆★☆★各種パラメータ調整箇所☆★☆★☆★
yusaku0125 5:f635f1f01d2d 14 #define DEFAULT_SPEED 500 //機体の直進速度1000[mm/s]
yusaku0125 3:e455433c8cae 15 #define PULSE_TO_UM 30 //エンコーダ1パルス当たりのタイヤ移動距離[um]
yusaku0125 4:ac9e6772ddb3 16 #define INTERRUPT_TIME 1000 //割りこみ周期[us]
yusaku0125 4:ac9e6772ddb3 17
yusaku0125 5:f635f1f01d2d 18 //モータ速度のゲイン関連(むやみに調整しない)
yusaku0125 5:f635f1f01d2d 19 #define M_KP 0.002f//P(比例)制御成分
yusaku0125 5:f635f1f01d2d 20 #define M_KD 0.001f//D(微分)制御成分
yusaku0125 5:f635f1f01d2d 21
yusaku0125 5:f635f1f01d2d 22 //フォトリフレクタのゲイン（外側に行くにつれ値を何倍させたいか調整する。）
yusaku0125 5:f635f1f01d2d 23 #define S_K1 1.0f //float演算させる値には必ずfを付ける
yusaku0125 5:f635f1f01d2d 24 #define S_K2 2.0f //2倍
yusaku0125 5:f635f1f01d2d 25 #define S_K3 4.0f //4倍
yusaku0125 5:f635f1f01d2d 26
yusaku0125 5:f635f1f01d2d 27 //ラインセンサ各種制御成分
yusaku0125 5:f635f1f01d2d 28 #define S_KP 0.5f //ラインセンサ比例成分。大きいほど曲がりやすい
yusaku0125 5:f635f1f01d2d 29 #define S_KD 0.3f //ラインセンサ微分成分。大きいほど急なラインずれに強くなる。
yusaku0125 5:f635f1f01d2d 30
yusaku0125 4:ac9e6772ddb3 31
yusaku0125 4:ac9e6772ddb3 32 //////////☆★☆★☆★☆★☆★//////////////
yusaku0125 4:ac9e6772ddb3 33
yusaku0125 5:f635f1f01d2d 34 /////アナログ入力オブジェクト定義//////////
yusaku0125 5:f635f1f01d2d 35 AnalogIn s1(D3);
yusaku0125 5:f635f1f01d2d 36 AnalogIn s2(A6);
yusaku0125 5:f635f1f01d2d 37 AnalogIn s3(A5);
yusaku0125 5:f635f1f01d2d 38 AnalogIn s4(A4);
yusaku0125 5:f635f1f01d2d 39 AnalogIn s5(A3);
yusaku0125 5:f635f1f01d2d 40 AnalogIn s6(A2);
yusaku0125 5:f635f1f01d2d 41 AnalogIn s7(A1);
yusaku0125 5:f635f1f01d2d 42 AnalogIn s8(A0);
yusaku0125 5:f635f1f01d2d 43 ///////////////////////////////////////
yusaku0125 3:e455433c8cae 44 Serial PC(USBTX,USBRX);
yusaku0125 3:e455433c8cae 45 CRotaryEncoder encoder_a(D1,D0); //モータAのエンコーダ
yusaku0125 3:e455433c8cae 46 CRotaryEncoder encoder_b(D11,D12); //モータBのエンコーダ
yusaku0125 3:e455433c8cae 47 Ticker timer; //タイマ割込み用
yusaku0125 4:ac9e6772ddb3 48 TB6612 motor_a(D2,D7,D6); //モータA制御用(pwma,ain1,ain2)
yusaku0125 4:ac9e6772ddb3 49 TB6612 motor_b(D10,D8,D9); //モータB制御用(pwmb,bin1,bin2)
yusaku0125 4:ac9e6772ddb3 50
yusaku0125 4:ac9e6772ddb3 51
yusaku0125 5:f635f1f01d2d 52 //使用変数の定義
yusaku0125 5:f635f1f01d2d 53 float S1_Data,S2_Data,S3_Data,S4_Data,S5_Data,S6_Data,S7_Data,S8_Data;
yusaku0125 5:f635f1f01d2d 54 float All_Sensor_Data; //ラインセンサ総データ量
yusaku0125 5:f635f1f01d2d 55 float Sensor_Diff[2]={0,0}; //ラインセンサ偏差
yusaku0125 5:f635f1f01d2d 56 float Sensor_P=0.0f; //ラインセンサP(比例成分)制御量
yusaku0125 5:f635f1f01d2d 57 float Sensor_D=0.0f; //ラインセンサD(微分成分)制御量
yusaku0125 5:f635f1f01d2d 58 float Sensor_PD=0.0f; //ラインセンサP,D成分の合計
yusaku0125 5:f635f1f01d2d 59 long int Enc_Count_A=0,Enc_Count_B=0; //エンコーダパルス数を格納
yusaku0125 5:f635f1f01d2d 60 long int Distance_A=0,Distance_B=0; //タイヤ移動距離を格納[mm]
yusaku0125 5:f635f1f01d2d 61 long int Speed_A=0, Speed_B=0; //現在速度
yusaku0125 5:f635f1f01d2d 62 long int Target_Speed_A=0,Target_Speed_B=0; //目標速度
yusaku0125 5:f635f1f01d2d 63 long int Motor_A_Diff[2]={0,0}; //過去の偏差と現在の偏差を格納
yusaku0125 5:f635f1f01d2d 64 long int Motor_B_Diff[2]={0,0};
yusaku0125 4:ac9e6772ddb3 65 float Motor_A_P,Motor_B_P; //モータP制御成分
yusaku0125 4:ac9e6772ddb3 66 float Motor_A_D,Motor_B_D; //モータD制御成分
yusaku0125 4:ac9e6772ddb3 67 float Motor_A_Pwm,Motor_B_Pwm; //モータへの出力
yusaku0125 3:e455433c8cae 68
yusaku0125 4:ac9e6772ddb3 69 void timer_interrupt(){
yusaku0125 5:f635f1f01d2d 70 Sensor_Diff[1]=Sensor_Diff[0];//過去のラインセンサ偏差を退避
yusaku0125 5:f635f1f01d2d 71 //各種センサ情報取得
yusaku0125 5:f635f1f01d2d 72 S1_Data=s1.read();
yusaku0125 5:f635f1f01d2d 73 S2_Data=s2.read();
yusaku0125 5:f635f1f01d2d 74 S3_Data=s3.read();
yusaku0125 5:f635f1f01d2d 75 S4_Data=s4.read();
yusaku0125 5:f635f1f01d2d 76 S5_Data=s5.read();
yusaku0125 5:f635f1f01d2d 77 S6_Data=s6.read();
yusaku0125 5:f635f1f01d2d 78 S7_Data=s7.read();
yusaku0125 5:f635f1f01d2d 79 S8_Data=s8.read();
yusaku0125 5:f635f1f01d2d 80 //センサ取得値の重ね合わせ（端のセンサほどモータ制御量を大きくする）
yusaku0125 5:f635f1f01d2d 81 All_Sensor_Data=-(S2_Data*S_K3+S3_Data*S_K2+S4_Data*S_K1)+(S5_Data*S_K1+S6_Data*S_K2+S7_Data*S_K3);
yusaku0125 5:f635f1f01d2d 82 Sensor_Diff[0]=All_Sensor_Data;
yusaku0125 5:f635f1f01d2d 83 Sensor_P=All_Sensor_Data*S_KP; //ラインセンサ比例成分の演算
yusaku0125 5:f635f1f01d2d 84 Sensor_D=(Sensor_Diff[0]-Sensor_Diff[1])*S_KD; //ラインセンサ微分成分の演算
yusaku0125 5:f635f1f01d2d 85 Sensor_PD=Sensor_P+Sensor_D;
yusaku0125 4:ac9e6772ddb3 86 ////モータ現在速度の取得
yusaku0125 4:ac9e6772ddb3 87 Enc_Count_A=encoder_a.Get(); //エンコーダパルス数を取得
yusaku0125 4:ac9e6772ddb3 88 Enc_Count_B=-encoder_b.Get();
yusaku0125 4:ac9e6772ddb3 89 Distance_A=(Enc_Count_A*PULSE_TO_UM); //移動距離をmm単位で格納
yusaku0125 4:ac9e6772ddb3 90 Distance_B=(Enc_Count_B*PULSE_TO_UM);
yusaku0125 4:ac9e6772ddb3 91 Speed_A=(Distance_A*1000)/INTERRUPT_TIME;//走行速度演算[mm/s]
yusaku0125 4:ac9e6772ddb3 92 Speed_B=(Distance_B*1000)/INTERRUPT_TIME;
yusaku0125 4:ac9e6772ddb3 93 Distance_A=0;
yusaku0125 4:ac9e6772ddb3 94 Distance_B=0;
yusaku0125 3:e455433c8cae 95 encoder_a.Set(0);
yusaku0125 4:ac9e6772ddb3 96 encoder_b.Set(0);
yusaku0125 4:ac9e6772ddb3 97
yusaku0125 4:ac9e6772ddb3 98 /////各モータの目標速度の設定
yusaku0125 4:ac9e6772ddb3 99 Target_Speed_A=DEFAULT_SPEED;
yusaku0125 4:ac9e6772ddb3 100 Target_Speed_B=DEFAULT_SPEED;
yusaku0125 4:ac9e6772ddb3 101
yusaku0125 4:ac9e6772ddb3 102 /////モータの速度制御
yusaku0125 4:ac9e6772ddb3 103 //過去の速度偏差を退避
yusaku0125 4:ac9e6772ddb3 104 Motor_A_Diff[1]=Motor_A_Diff[0];
yusaku0125 4:ac9e6772ddb3 105 Motor_A_Diff[1]=Motor_A_Diff[0];
yusaku0125 4:ac9e6772ddb3 106 //現在の速度偏差を取得。
yusaku0125 4:ac9e6772ddb3 107 Motor_A_Diff[0]=(Target_Speed_A-Speed_A);
yusaku0125 4:ac9e6772ddb3 108 Motor_B_Diff[0]=(Target_Speed_B-Speed_B);
yusaku0125 4:ac9e6772ddb3 109 //P成分演算
yusaku0125 4:ac9e6772ddb3 110 Motor_A_P=Motor_A_Diff[0]*M_KP;
yusaku0125 4:ac9e6772ddb3 111 Motor_B_P=Motor_B_Diff[0]*M_KP;
yusaku0125 4:ac9e6772ddb3 112 //D成分演算
yusaku0125 4:ac9e6772ddb3 113 Motor_A_D=(Motor_A_Diff[0]-Motor_A_Diff[1])*M_KD;
yusaku0125 4:ac9e6772ddb3 114 Motor_B_D=(Motor_B_Diff[0]-Motor_B_Diff[1])*M_KD;
yusaku0125 4:ac9e6772ddb3 115
yusaku0125 5:f635f1f01d2d 116 Motor_A_Pwm=Motor_A_P+Motor_A_D+Sensor_PD;
yusaku0125 5:f635f1f01d2d 117 Motor_B_Pwm=Motor_B_P+Motor_B_D-Sensor_PD;
yusaku0125 5:f635f1f01d2d 118 if(Motor_A_Pwm>1.0f)Motor_A_Pwm=1.0f;
yusaku0125 5:f635f1f01d2d 119 else if(Motor_A_Pwm<-1.0f)Motor_A_Pwm=-1.0f;
yusaku0125 5:f635f1f01d2d 120 if(Motor_B_Pwm>1.0f)Motor_B_Pwm=1.0f;
yusaku0125 5:f635f1f01d2d 121 else if(Motor_B_Pwm<-1.0f)Motor_B_Pwm=-1.0f;
yusaku0125 4:ac9e6772ddb3 122 //最終的には符号を逆転して出力
yusaku0125 4:ac9e6772ddb3 123 motor_a=-Motor_A_Pwm;
yusaku0125 4:ac9e6772ddb3 124 motor_b=-Motor_B_Pwm;
yusaku0125 3:e455433c8cae 125 }
yusaku0125 3:e455433c8cae 126
yusaku0125 3:e455433c8cae 127 int main() {
yusaku0125 3:e455433c8cae 128 timer.attach_us(&timer_interrupt,INTERRUPT_TIME);//タイマ割り込みスタート
yusaku0125 3:e455433c8cae 129 while(1){
yusaku0125 3:e455433c8cae 130 wait(1);
yusaku0125 4:ac9e6772ddb3 131 PC.printf("spd_a:%d[mm/sec] spd_b:%d[mm/sec]\r\n",Speed_A,Speed_B);//表示
yusaku0125 0:df99e50ed3fd 132 }
yusaku0125 3:e455433c8cae 133 }