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

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

Dependencies:   mbed i2c_gyro_mpu_6050 AQM0802 TB6612FNG

main.cpp@3:623a84032d43, 2019-08-05 (annotated)

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