玉田機体ピン配置がサンプルとのピン配置が違う
Dependencies: mbed i2c_gyro_mpu_6050 AQM0802 TB6612FNG
main.cpp
- Committer:
- yusaku0125
- Date:
- 2019-08-21
- Revision:
- 4:c88cf4e101e2
- Parent:
- 3:623a84032d43
- Child:
- 5:2f1098259948
File content as of revision 4:c88cf4e101e2:
//モータドライバTB6612動作検証用のサンプルプログラム //TB6612クラスを使用して、モータA,モータBのオブジェクトを生成する。 //生成時のピン割当はマイコンピン割当通りに配置すること。 //機体がきれいにライントレースするように各種パラメータ調整を行いこと。 #include "mbed.h" #include "TB6612.h" #include "AQM0802.h" #include "mpu6050.h" AQM0802 lcd(D4,D5); MPU6050 mpu(D4, D5); TB6612 motor_a(D2,D7,D6); //モータA制御用(pwma,ain1,ain2) TB6612 motor_b(D10,D8,D9); //モータB制御用(pwmb,bin1,bin2) Serial pc(USBTX,USBRX); //USBシリアル通信用 AnalogIn s1(D3); AnalogIn s2(A6); AnalogIn s3(A5); AnalogIn s4(A4); AnalogIn s5(A3); AnalogIn s6(A2); AnalogIn s7(A1); AnalogIn s8(A0); //☆★☆★各種パラメータ調整箇所☆★☆★☆★ #define DEFAULT_SPEED 0.3f //機体の直進速度30% //フォトリフレクタのゲイン(外側に行くにつれ値を何倍させたいか調整する。) #define S_K1 1.0f //float演算させる値には必ずfを付ける #define S_K2 2.0f //2倍 #define S_K3 4.0f //4倍 //ラインセンサ比例制御成分 #define S_KP 0.3f //ラインセンサによるモータ制御量 //大きいほど曲がりやすい //ジャイロセンサのゲイン //#define G_KP 0.75f #define G_KP 0.0f //ジャイロ未使用の場合は0.0(制御量無し)にしておく //////////////////////////////////////////////////////////////// //使用変数の定義 float S1_Data,S2_Data,S3_Data,S4_Data,S5_Data,S6_Data,S7_Data,S8_Data; float All_Sensor_Data; float Sensor_P; float Motor_A_Pwm,Motor_B_Pwm; char Str_S[4],Str_G[4]; double Gx,Gy,Gz; //Z角速度情報を格納 float Gz_F; //Z軸データint型変換格納 float Gyro_P; //ジャイロセンサP成分 int main() { lcd.init(); //LCD初期化 lcd.locate(0,0); //桁、行 mpu.setMaxScale(MAX_ACC_8G, MAX_GYRO_1000degpersec);//[1000deg/sec]を測定上限とする。 while(1) { wait_ms(10); //各種センサ情報取得 S1_Data=s1.read(); S2_Data=s2.read(); S3_Data=s3.read(); S4_Data=s4.read(); S5_Data=s5.read(); S6_Data=s6.read(); S7_Data=s7.read(); S8_Data=s8.read(); //★☆角速度情報の取得☆★ mpu.readGyroscope(Gx, Gy, Gz);//関数仕様上3軸すべて角速度取得する。 Gz_F=(float)Gz/1000; //doubleは大きすぎるのでfloat型へ変換 Gyro_P=Gz_F*G_KP; //センサ取得値の重ね合わせ(端のセンサほどモータ制御量を大きくする) 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); Sensor_P=All_Sensor_Data*S_KP;//センサ比例成分の演算 //LCD1行目にセンサ合計値の表示 lcd.locate(0,0);//桁、行 lcd.print(" ");//表示クリア lcd.locate(0,0); lcd.print("S:"); sprintf(Str_S,"%+1.2f",Sensor_P);//センサ比例成分を文字列変換 lcd.print(Str_S); //★☆LCDへの角速度表示☆★ lcd.locate(0,1);//桁、行 lcd.print(" ");//表示クリア lcd.locate(0,1); lcd.print("G:"); sprintf(Str_G,"%+1.2f",Gz_F); lcd.print(Str_G); //モータ制御量の演算 Motor_A_Pwm=DEFAULT_SPEED + Sensor_P - Gyro_P; Motor_B_Pwm=DEFAULT_SPEED - Sensor_P + Gyro_P; //モータ出力は±1.0が上下限度なので限界値を設定する。 if(Motor_A_Pwm> 1.0f)Motor_A_Pwm=1.0f; else if(Motor_A_Pwm< -1.0f)Motor_A_Pwm=-1.0f; if(Motor_B_Pwm> 1.0f)Motor_B_Pwm=1.0f; else if(Motor_B_Pwm< -1.0f)Motor_B_Pwm=-1.0f; //最終的には符号を逆転して出力 motor_a=-Motor_A_Pwm; motor_b=-Motor_B_Pwm; } }