![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
タイマ割り込みを用いたライントレース。 割り込み処理内でI2Cを使用することができないので、 LCD,ジャイロ関連のプログラムは省略。
Dependencies: mbed i2c_gyro_mpu_6050 AQM0802 TB6612FNG
main.cpp
- Committer:
- yusaku0125
- Date:
- 2019-08-26
- Revision:
- 5:27963aaa8f4a
- Parent:
- 4:c88cf4e101e2
File content as of revision 5:27963aaa8f4a:
//モータドライバTB6612動作検証用のサンプルプログラム //TB6612クラスを使用して、モータA,モータBのオブジェクトを生成する。 //生成時のピン割当はマイコンピン割当通りに配置すること。 //機体がきれいにライントレースするように各種パラメータ調整を行いこと。 #include "mbed.h" #include "TB6612.h" #include "AQM0802.h" #include "mpu6050.h" 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); 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 //ラインセンサによるモータ制御量 //大きいほど曲がりやすい //////////////////////////////////////////////////////////////// //使用変数の定義 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; void timer_interrupt(){ //各種センサ情報取得 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(); //センサ取得値の重ね合わせ(端のセンサほどモータ制御量を大きくする) 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;//センサ比例成分の演算 //モータ制御量の演算 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; 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; } int main() { timer.attach_us(&timer_interrupt,1000);//1ms単位 while(1) { } }