玉田機体ピン配置がサンプルとのピン配置が違う
Dependencies: mbed i2c_gyro_mpu_6050 AQM0802 TB6612FNG
main.cpp@6:cc3de88408bb, 2019-08-26 (annotated)
- Committer:
- GGU
- Date:
- Mon Aug 26 04:38:02 2019 +0000
- Revision:
- 6:cc3de88408bb
- Parent:
- 5:2f1098259948
test
Who changed what in which revision?
User | Revision | Line number | New 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 | 6:cc3de88408bb | 19 | AnalogIn s1(D3); |
GGU | 6:cc3de88408bb | 20 | AnalogIn s2(A6); |
GGU | 6:cc3de88408bb | 21 | AnalogIn s3(A5); |
GGU | 6:cc3de88408bb | 22 | AnalogIn s4(A4); |
GGU | 6:cc3de88408bb | 23 | AnalogIn s5(A3); |
GGU | 6:cc3de88408bb | 24 | AnalogIn s6(A2); |
GGU | 6:cc3de88408bb | 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 | |
GGU | 5:2f1098259948 | 33 | #define DEFAULT_SPEED 0.15f //機体の直進速度30% |
yusaku0125 | 4:c88cf4e101e2 | 34 | |
GGU | 5:2f1098259948 | 35 | //フォトリフレクタのゲイン(外側に行くにつれ値を何倍させたいか調整する。 |
GGU | 6:cc3de88408bb | 36 | #define S_K1 1.2f //float演算させる値には必ずfを付ける |
GGU | 6:cc3de88408bb | 37 | #define S_K2 2.4f //2倍 |
GGU | 6:cc3de88408bb | 38 | #define S_K3 3.9f //4倍 |
yusaku0125 | 4:c88cf4e101e2 | 39 | |
yusaku0125 | 4:c88cf4e101e2 | 40 | //ラインセンサ比例制御成分 |
GGU | 5:2f1098259948 | 41 | #define S_KP 0.07f //ラインセンサによるモータ制御量 |
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; //Z軸データ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);//関数仕様上3軸すべて角速度取得する。 |
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 | } |