タイマ割り込みを用いたライントレース。 割り込み処理内でI2Cを使用することができないので、 LCD,ジャイロ関連のプログラムは省略。
Dependencies: mbed i2c_gyro_mpu_6050 AQM0802 TB6612FNG
Diff: main.cpp
- Revision:
- 3:623a84032d43
- Parent:
- 2:7ca124b4dbcb
- Child:
- 4:c88cf4e101e2
--- a/main.cpp Mon Jul 22 10:28:52 2019 +0000 +++ b/main.cpp Mon Aug 05 10:01:04 2019 +0000 @@ -3,32 +3,101 @@ //生成時のピン割当はマイコンピン割当通りに配置すること。 #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 S_K1 1.0f //float演算させる値には必ずfを付ける +#define S_K2 2.0f +#define S_K3 4.0f +#define S_KP 0.3f + +//ジャイロセンサのゲイン +#define G_KP 0.75f + +#define DEFAULT_SPEED 0.3f + +//使用変数の定義 +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() { - float motor_speed; //モータスピード情報格納用 - char input_data; //キーボード入力情報格納用 + lcd.init(); //LCD初期化 + lcd.locate(0,0); //桁、行 + mpu.setMaxScale(MAX_ACC_8G, MAX_GYRO_1000degpersec);//[1000deg/sec]を測定上限とする。 + + while(1) { - input_data=pc.getc(); //キーボード入力情報取得 - motor_speed=0.5; //モータスピード(低速運転させるため2分の1の値とする。) - switch(input_data){ - case '1': motor_a=motor_speed; //モータA正転 - break; - case '2': motor_a=0; //モータAブレーキ - break; - case '3': motor_a=-motor_speed; //モータA逆転 - break; - case '7': motor_b=motor_speed; //モータB正転 - break; - case '8': motor_b=0; //モータBブレーキ - break; - case '9': motor_b=-motor_speed; //モータB正転 - break; - default : motor_a=0; - motor_b=0; //両方モータブレーキ - break; - } + 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; + } }