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

ライントレース基本プログラム フォトリフレクタによる制御量と ジャイロセンサによる制御量をLCDに表示。

Dependencies:   mbed i2c_gyro_mpu_6050 AQM0802 TB6612FNG

main.cpp@4:c88cf4e101e2, 2019-08-21 (annotated)

Committer:
yusaku0125
Date:
Wed Aug 21 01:34:17 2019 +0000
Revision:
4:c88cf4e101e2
Parent:
3:623a84032d43 
test

Who changed what in which revision?

UserRevisionLine numberNew contents of line
yusaku0125 0:7767f80cd27b 1 //モータドライバTB6612動作検証用のサンプルプログラム
yusaku0125 0:7767f80cd27b 2 //TB6612クラスを使用して、モータA,モータBのオブジェクトを生成する。
yusaku0125 1:b71b5722e3a1 3 //生成時のピン割当はマイコンピン割当通りに配置すること。
yusaku0125 4:c88cf4e101e2 4
yusaku0125 4:c88cf4e101e2 5 //機体がきれいにライントレースするように各種パラメータ調整を行いこと。
yusaku0125 4:c88cf4e101e2 6
yusaku0125 4:c88cf4e101e2 7
yusaku0125 0:7767f80cd27b 8 #include "mbed.h"
yusaku0125 0:7767f80cd27b 9 #include "TB6612.h"
GGU 3:623a84032d43 10 #include "AQM0802.h"
GGU 3:623a84032d43 11 #include "mpu6050.h"
GGU 3:623a84032d43 12 AQM0802 lcd(D4,D5);
GGU 3:623a84032d43 13 MPU6050 mpu(D4, D5);
yusaku0125 1:b71b5722e3a1 14 TB6612 motor_a(D2,D7,D6); //モータA制御用(pwma,ain1,ain2)
yusaku0125 1:b71b5722e3a1 15 TB6612 motor_b(D10,D8,D9); //モータB制御用(pwmb,bin1,bin2)
GGU 3:623a84032d43 16
yusaku0125 0:7767f80cd27b 17 Serial pc(USBTX,USBRX); //USBシリアル通信用
yusaku0125 0:7767f80cd27b 18
GGU 3:623a84032d43 19 AnalogIn s1(D3);
GGU 3:623a84032d43 20 AnalogIn s2(A6);
GGU 3:623a84032d43 21 AnalogIn s3(A5);
GGU 3:623a84032d43 22 AnalogIn s4(A4);
GGU 3:623a84032d43 23 AnalogIn s5(A3);
GGU 3:623a84032d43 24 AnalogIn s6(A2);
GGU 3:623a84032d43 25 AnalogIn s7(A1);
GGU 3:623a84032d43 26 AnalogIn s8(A0);
GGU 3:623a84032d43 27
GGU 3:623a84032d43 28
yusaku0125 4:c88cf4e101e2 29
yusaku0125 4:c88cf4e101e2 30
yusaku0125 4:c88cf4e101e2 31 //☆★☆★各種パラメータ調整箇所☆★☆★☆★
yusaku0125 4:c88cf4e101e2 32
yusaku0125 4:c88cf4e101e2 33 #define DEFAULT_SPEED 0.3f //機体の直進速度30％
yusaku0125 4:c88cf4e101e2 34
yusaku0125 4:c88cf4e101e2 35 //フォトリフレクタのゲイン（外側に行くにつれ値を何倍させたいか調整する。）
GGU 3:623a84032d43 36 #define S_K1 1.0f //float演算させる値には必ずfを付ける
yusaku0125 4:c88cf4e101e2 37 #define S_K2 2.0f //2倍
yusaku0125 4:c88cf4e101e2 38 #define S_K3 4.0f //4倍
yusaku0125 4:c88cf4e101e2 39
yusaku0125 4:c88cf4e101e2 40 //ラインセンサ比例制御成分
yusaku0125 4:c88cf4e101e2 41 #define S_KP 0.3f //ラインセンサによるモータ制御量
yusaku0125 4:c88cf4e101e2 42 //大きいほど曲がりやすい
GGU 3:623a84032d43 43
GGU 3:623a84032d43 44 //ジャイロセンサのゲイン
yusaku0125 4:c88cf4e101e2 45 //#define G_KP 0.75f
yusaku0125 4:c88cf4e101e2 46 #define G_KP 0.0f //ジャイロ未使用の場合は0.0(制御量無し)にしておく
yusaku0125 4:c88cf4e101e2 47
yusaku0125 4:c88cf4e101e2 48 ////////////////////////////////////////////////////////////////
GGU 3:623a84032d43 49
yusaku0125 4:c88cf4e101e2 50
yusaku0125 4:c88cf4e101e2 51
yusaku0125 4:c88cf4e101e2 52
yusaku0125 4:c88cf4e101e2 53
GGU 3:623a84032d43 54
GGU 3:623a84032d43 55 //使用変数の定義
GGU 3:623a84032d43 56 float S1_Data,S2_Data,S3_Data,S4_Data,S5_Data,S6_Data,S7_Data,S8_Data;
GGU 3:623a84032d43 57 float All_Sensor_Data;
GGU 3:623a84032d43 58 float Sensor_P;
GGU 3:623a84032d43 59 float Motor_A_Pwm,Motor_B_Pwm;
GGU 3:623a84032d43 60 char Str_S[4],Str_G[4];
GGU 3:623a84032d43 61 double Gx,Gy,Gz; //Z角速度情報を格納
GGU 3:623a84032d43 62 float Gz_F; //Ｚ軸データint型変換格納
GGU 3:623a84032d43 63 float Gyro_P; //ジャイロセンサP成分
GGU 3:623a84032d43 64
yusaku0125 0:7767f80cd27b 65 int main() {
GGU 3:623a84032d43 66 lcd.init(); //LCD初期化
GGU 3:623a84032d43 67 lcd.locate(0,0); //桁、行
GGU 3:623a84032d43 68 mpu.setMaxScale(MAX_ACC_8G, MAX_GYRO_1000degpersec);//[1000deg/sec]を測定上限とする。
GGU 3:623a84032d43 69
GGU 3:623a84032d43 70
yusaku0125 0:7767f80cd27b 71 while(1) {
GGU 3:623a84032d43 72 wait_ms(10);
GGU 3:623a84032d43 73 //各種センサ情報取得
GGU 3:623a84032d43 74 S1_Data=s1.read();
GGU 3:623a84032d43 75 S2_Data=s2.read();
GGU 3:623a84032d43 76 S3_Data=s3.read();
GGU 3:623a84032d43 77 S4_Data=s4.read();
GGU 3:623a84032d43 78 S5_Data=s5.read();
GGU 3:623a84032d43 79 S6_Data=s6.read();
GGU 3:623a84032d43 80 S7_Data=s7.read();
GGU 3:623a84032d43 81 S8_Data=s8.read();
GGU 3:623a84032d43 82
GGU 3:623a84032d43 83 //★☆角速度情報の取得☆★
GGU 3:623a84032d43 84 mpu.readGyroscope(Gx, Gy, Gz);//関数仕様上３軸すべて角速度取得する。
GGU 3:623a84032d43 85 Gz_F=(float)Gz/1000; //doubleは大きすぎるのでfloat型へ変換
GGU 3:623a84032d43 86 Gyro_P=Gz_F*G_KP;
GGU 3:623a84032d43 87
GGU 3:623a84032d43 88 //センサ取得値の重ね合わせ（端のセンサほどモータ制御量を大きくする）
GGU 3:623a84032d43 89 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);
GGU 3:623a84032d43 90 Sensor_P=All_Sensor_Data*S_KP;//センサ比例成分の演算
GGU 3:623a84032d43 91
GGU 3:623a84032d43 92 //LCD1行目にセンサ合計値の表示
GGU 3:623a84032d43 93 lcd.locate(0,0);//桁、行
GGU 3:623a84032d43 94 lcd.print(" ");//表示クリア
GGU 3:623a84032d43 95 lcd.locate(0,0);
GGU 3:623a84032d43 96 lcd.print("S:");
GGU 3:623a84032d43 97 sprintf(Str_S,"%+1.2f",Sensor_P);//センサ比例成分を文字列変換
GGU 3:623a84032d43 98 lcd.print(Str_S);
GGU 3:623a84032d43 99
GGU 3:623a84032d43 100 //★☆LCDへの角速度表示☆★
GGU 3:623a84032d43 101 lcd.locate(0,1);//桁、行
GGU 3:623a84032d43 102 lcd.print(" ");//表示クリア
GGU 3:623a84032d43 103 lcd.locate(0,1);
GGU 3:623a84032d43 104 lcd.print("G:");
GGU 3:623a84032d43 105 sprintf(Str_G,"%+1.2f",Gz_F);
GGU 3:623a84032d43 106 lcd.print(Str_G);
GGU 3:623a84032d43 107
GGU 3:623a84032d43 108 //モータ制御量の演算
GGU 3:623a84032d43 109 Motor_A_Pwm=DEFAULT_SPEED + Sensor_P - Gyro_P;
GGU 3:623a84032d43 110 Motor_B_Pwm=DEFAULT_SPEED - Sensor_P + Gyro_P;
GGU 3:623a84032d43 111 //モータ出力は±1.0が上下限度なので限界値を設定する。
GGU 3:623a84032d43 112 if(Motor_A_Pwm> 1.0f)Motor_A_Pwm=1.0f;
GGU 3:623a84032d43 113 else if(Motor_A_Pwm< -1.0f)Motor_A_Pwm=-1.0f;
GGU 3:623a84032d43 114 if(Motor_B_Pwm> 1.0f)Motor_B_Pwm=1.0f;
GGU 3:623a84032d43 115 else if(Motor_B_Pwm< -1.0f)Motor_B_Pwm=-1.0f;
GGU 3:623a84032d43 116
GGU 3:623a84032d43 117 //最終的には符号を逆転して出力
GGU 3:623a84032d43 118 motor_a=-Motor_A_Pwm;
GGU 3:623a84032d43 119 motor_b=-Motor_B_Pwm;
GGU 3:623a84032d43 120
yusaku0125 0:7767f80cd27b 121 }
yusaku0125 0:7767f80cd27b 122 }