![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
タイマ割り込みを用いたライントレース。 割り込み処理内でI2Cを使用することができないので、 LCD,ジャイロ関連のプログラムは省略。
Dependencies: mbed i2c_gyro_mpu_6050 AQM0802 TB6612FNG
Revision 5:27963aaa8f4a, committed 2019-08-26
- Comitter:
- yusaku0125
- Date:
- Mon Aug 26 04:25:40 2019 +0000
- Parent:
- 4:c88cf4e101e2
- Commit message:
- timer_interrupt
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Wed Aug 21 01:34:17 2019 +0000 +++ b/main.cpp Mon Aug 26 04:25:40 2019 +0000 @@ -9,11 +9,9 @@ #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) - +Ticker timer; //タイマ割込み用 Serial pc(USBTX,USBRX); //USBシリアル通信用 AnalogIn s1(D3); @@ -40,37 +38,17 @@ //ラインセンサ比例制御成分 #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); - //各種センサ情報取得 +void timer_interrupt(){ + //各種センサ情報取得 S1_Data=s1.read(); S2_Data=s2.read(); S3_Data=s3.read(); @@ -79,35 +57,16 @@ 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; + Motor_A_Pwm=DEFAULT_SPEED + Sensor_P; + Motor_B_Pwm=DEFAULT_SPEED - Sensor_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; @@ -116,7 +75,11 @@ //最終的には符号を逆転して出力 motor_a=-Motor_A_Pwm; - motor_b=-Motor_B_Pwm; + motor_b=-Motor_B_Pwm; +} +int main() { + timer.attach_us(&timer_interrupt,1000);//1ms単位 + while(1) { } }