main.cpp

00001 //モータドライバTB6612動作検証用のサンプルプログラム
00002 //TB6612クラスを使用して、モータA,モータBのオブジェクトを生成する。
00003 //生成時のピン割当はマイコンピン割当通りに配置すること。
00004 
00005 //機体がきれいにライントレースするように各種パラメータ調整を行いこと。
00006 
00007 
00008 #include "mbed.h"
00009 #include "TB6612.h"
00010 #include "AQM0802.h"
00011 #include "mpu6050.h"
00012 AQM0802 lcd(D4,D5);
00013 MPU6050 mpu(D4, D5);
00014 TB6612      motor_a(D2,D7,D6);  //モータA制御用(pwma,ain1,ain2)
00015 TB6612      motor_b(D10,D8,D9);  //モータB制御用(pwmb,bin1,bin2)
00016 
00017 Serial      pc(USBTX,USBRX);    //USBシリアル通信用
00018 
00019 AnalogIn s1(D3);
00020 AnalogIn s2(A6);
00021 AnalogIn s3(A5);
00022 AnalogIn s4(A4);
00023 AnalogIn s5(A3);
00024 AnalogIn s6(A2);
00025 AnalogIn s7(A1);
00026 AnalogIn s8(A0);
00027 
00028 
00029 
00030 
00031 //☆★☆★各種パラメータ調整箇所☆★☆★☆★
00032 
00033 #define DEFAULT_SPEED 0.3f   //機体の直進速度30％
00034 
00035 //フォトリフレクタのゲイン（外側に行くにつれ値を何倍させたいか調整する。）
00036 #define S_K1    1.0f    //float演算させる値には必ずfを付ける
00037 #define S_K2    2.0f    //2倍
00038 #define S_K3    4.0f    //4倍
00039 
00040 //ラインセンサ比例制御成分
00041 #define S_KP    0.3f    //ラインセンサによるモータ制御量
00042                         //大きいほど曲がりやすい
00043 
00044 //ジャイロセンサのゲイン
00045 //#define G_KP    0.75f
00046 #define G_KP    0.0f    //ジャイロ未使用の場合は0.0(制御量無し)にしておく
00047 
00048 ////////////////////////////////////////////////////////////////
00049 
00050 
00051 
00052 
00053 
00054 
00055 //使用変数の定義
00056 float S1_Data,S2_Data,S3_Data,S4_Data,S5_Data,S6_Data,S7_Data,S8_Data; 
00057 float All_Sensor_Data;
00058 float Sensor_P;
00059 float Motor_A_Pwm,Motor_B_Pwm;
00060 char  Str_S[4],Str_G[4];
00061 double Gx,Gy,Gz;    //Z角速度情報を格納
00062 float Gz_F;         //Ｚ軸データint型変換格納
00063 float Gyro_P;       //ジャイロセンサP成分
00064 
00065 int main() {
00066     lcd.init();         //LCD初期化
00067     lcd.locate(0,0);    //桁、行    
00068     mpu.setMaxScale(MAX_ACC_8G, MAX_GYRO_1000degpersec);//[1000deg/sec]を測定上限とする。
00069     
00070          
00071     while(1) {
00072        wait_ms(10);
00073        //各種センサ情報取得
00074        S1_Data=s1.read();
00075        S2_Data=s2.read();
00076        S3_Data=s3.read();
00077        S4_Data=s4.read();
00078        S5_Data=s5.read();
00079        S6_Data=s6.read();
00080        S7_Data=s7.read();
00081        S8_Data=s8.read();
00082 
00083         //★☆角速度情報の取得☆★
00084         mpu.readGyroscope(Gx, Gy, Gz);//関数仕様上３軸すべて角速度取得する。
00085         Gz_F=(float)Gz/1000;   //doubleは大きすぎるのでfloat型へ変換  
00086         Gyro_P=Gz_F*G_KP;                  
00087         
00088        //センサ取得値の重ね合わせ（端のセンサほどモータ制御量を大きくする）
00089        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);
00090        Sensor_P=All_Sensor_Data*S_KP;//センサ比例成分の演算
00091 
00092         //LCD1行目にセンサ合計値の表示
00093        lcd.locate(0,0);//桁、行
00094        lcd.print("        ");//表示クリア
00095        lcd.locate(0,0);
00096        lcd.print("S:");
00097        sprintf(Str_S,"%+1.2f",Sensor_P);//センサ比例成分を文字列変換       
00098        lcd.print(Str_S);
00099  
00100         //★☆LCDへの角速度表示☆★        
00101         lcd.locate(0,1);//桁、行
00102         lcd.print("        ");//表示クリア
00103         lcd.locate(0,1);
00104         lcd.print("G:");
00105         sprintf(Str_G,"%+1.2f",Gz_F);
00106         lcd.print(Str_G);  
00107        
00108        //モータ制御量の演算
00109        Motor_A_Pwm=DEFAULT_SPEED + Sensor_P - Gyro_P;
00110        Motor_B_Pwm=DEFAULT_SPEED - Sensor_P + Gyro_P;       
00111        //モータ出力は±1.0が上下限度なので限界値を設定する。
00112        if(Motor_A_Pwm> 1.0f)Motor_A_Pwm=1.0f;
00113        else if(Motor_A_Pwm< -1.0f)Motor_A_Pwm=-1.0f;
00114        if(Motor_B_Pwm> 1.0f)Motor_B_Pwm=1.0f;
00115        else if(Motor_B_Pwm< -1.0f)Motor_B_Pwm=-1.0f;       
00116        
00117        //最終的には符号を逆転して出力
00118        motor_a=-Motor_A_Pwm;
00119        motor_b=-Motor_B_Pwm;
00120 
00121     }
00122 }