島根短大　ライントレーサ制作チーム / Mbed 2 deprecated line_trace_ver2

タイマ割り込みを用いたライントレース。 割り込み処理内でI2Cを使用することができないので、 LCD,ジャイロ関連のプログラムは省略。

Dependencies:   mbed i2c_gyro_mpu_6050 AQM0802 TB6612FNG

main.cpp@5:27963aaa8f4a, 2019-08-26 (annotated)

Committer:
yusaku0125
Date:
Mon Aug 26 04:25:40 2019 +0000
Revision:
5:27963aaa8f4a
Parent:
4:c88cf4e101e2 
timer_interrupt

Who changed what in which revision?

UserRevisionLine numberNew 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"
yusaku0125 1:b71b5722e3a1 12 TB6612 motor_a(D2,D7,D6); //モータA制御用(pwma,ain1,ain2)
yusaku0125 1:b71b5722e3a1 13 TB6612 motor_b(D10,D8,D9); //モータB制御用(pwmb,bin1,bin2)
yusaku0125 5:27963aaa8f4a 14 Ticker timer; //タイマ割込み用
yusaku0125 0:7767f80cd27b 15 Serial pc(USBTX,USBRX); //USBシリアル通信用
yusaku0125 0:7767f80cd27b 16
GGU 3:623a84032d43 17 AnalogIn s1(D3);
GGU 3:623a84032d43 18 AnalogIn s2(A6);
GGU 3:623a84032d43 19 AnalogIn s3(A5);
GGU 3:623a84032d43 20 AnalogIn s4(A4);
GGU 3:623a84032d43 21 AnalogIn s5(A3);
GGU 3:623a84032d43 22 AnalogIn s6(A2);
GGU 3:623a84032d43 23 AnalogIn s7(A1);
GGU 3:623a84032d43 24 AnalogIn s8(A0);
GGU 3:623a84032d43 25
GGU 3:623a84032d43 26
yusaku0125 4:c88cf4e101e2 27
yusaku0125 4:c88cf4e101e2 28
yusaku0125 4:c88cf4e101e2 29 //☆★☆★各種パラメータ調整箇所☆★☆★☆★
yusaku0125 4:c88cf4e101e2 30
yusaku0125 4:c88cf4e101e2 31 #define DEFAULT_SPEED 0.3f //機体の直進速度30％
yusaku0125 4:c88cf4e101e2 32
yusaku0125 4:c88cf4e101e2 33 //フォトリフレクタのゲイン（外側に行くにつれ値を何倍させたいか調整する。）
GGU 3:623a84032d43 34 #define S_K1 1.0f //float演算させる値には必ずfを付ける
yusaku0125 4:c88cf4e101e2 35 #define S_K2 2.0f //2倍
yusaku0125 4:c88cf4e101e2 36 #define S_K3 4.0f //4倍
yusaku0125 4:c88cf4e101e2 37
yusaku0125 4:c88cf4e101e2 38 //ラインセンサ比例制御成分
yusaku0125 4:c88cf4e101e2 39 #define S_KP 0.3f //ラインセンサによるモータ制御量
yusaku0125 4:c88cf4e101e2 40 //大きいほど曲がりやすい
yusaku0125 4:c88cf4e101e2 41 ////////////////////////////////////////////////////////////////
GGU 3:623a84032d43 42
yusaku0125 4:c88cf4e101e2 43
GGU 3:623a84032d43 44 //使用変数の定義
GGU 3:623a84032d43 45 float S1_Data,S2_Data,S3_Data,S4_Data,S5_Data,S6_Data,S7_Data,S8_Data;
GGU 3:623a84032d43 46 float All_Sensor_Data;
GGU 3:623a84032d43 47 float Sensor_P;
GGU 3:623a84032d43 48 float Motor_A_Pwm,Motor_B_Pwm;
GGU 3:623a84032d43 49
yusaku0125 5:27963aaa8f4a 50 void timer_interrupt(){
yusaku0125 5:27963aaa8f4a 51 //各種センサ情報取得
GGU 3:623a84032d43 52 S1_Data=s1.read();
GGU 3:623a84032d43 53 S2_Data=s2.read();
GGU 3:623a84032d43 54 S3_Data=s3.read();
GGU 3:623a84032d43 55 S4_Data=s4.read();
GGU 3:623a84032d43 56 S5_Data=s5.read();
GGU 3:623a84032d43 57 S6_Data=s6.read();
GGU 3:623a84032d43 58 S7_Data=s7.read();
GGU 3:623a84032d43 59 S8_Data=s8.read();
yusaku0125 5:27963aaa8f4a 60
GGU 3:623a84032d43 61
GGU 3:623a84032d43 62 //センサ取得値の重ね合わせ（端のセンサほどモータ制御量を大きくする）
GGU 3:623a84032d43 63 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 64 Sensor_P=All_Sensor_Data*S_KP;//センサ比例成分の演算
GGU 3:623a84032d43 65
GGU 3:623a84032d43 66
GGU 3:623a84032d43 67 //モータ制御量の演算
yusaku0125 5:27963aaa8f4a 68 Motor_A_Pwm=DEFAULT_SPEED + Sensor_P;
yusaku0125 5:27963aaa8f4a 69 Motor_B_Pwm=DEFAULT_SPEED - Sensor_P;
GGU 3:623a84032d43 70 //モータ出力は±1.0が上下限度なので限界値を設定する。
GGU 3:623a84032d43 71 if(Motor_A_Pwm> 1.0f)Motor_A_Pwm=1.0f;
GGU 3:623a84032d43 72 else if(Motor_A_Pwm< -1.0f)Motor_A_Pwm=-1.0f;
GGU 3:623a84032d43 73 if(Motor_B_Pwm> 1.0f)Motor_B_Pwm=1.0f;
GGU 3:623a84032d43 74 else if(Motor_B_Pwm< -1.0f)Motor_B_Pwm=-1.0f;
GGU 3:623a84032d43 75
GGU 3:623a84032d43 76 //最終的には符号を逆転して出力
GGU 3:623a84032d43 77 motor_a=-Motor_A_Pwm;
yusaku0125 5:27963aaa8f4a 78 motor_b=-Motor_B_Pwm;
yusaku0125 5:27963aaa8f4a 79 }
GGU 3:623a84032d43 80
yusaku0125 5:27963aaa8f4a 81 int main() {
yusaku0125 5:27963aaa8f4a 82 timer.attach_us(&timer_interrupt,1000);//1ms単位
yusaku0125 5:27963aaa8f4a 83 while(1) {
yusaku0125 0:7767f80cd27b 84 }
yusaku0125 0:7767f80cd27b 85 }