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 TB6612      motor_a(D2,D7,D6);  //モータA制御用(pwma,ain1,ain2)
00013 TB6612      motor_b(D10,D8,D9);  //モータB制御用(pwmb,bin1,bin2)
00014 Ticker      timer;     //タイマ割込み用
00015 Serial      pc(USBTX,USBRX);    //USBシリアル通信用
00016 
00017 AnalogIn s1(D3);
00018 AnalogIn s2(A6);
00019 AnalogIn s3(A5);
00020 AnalogIn s4(A4);
00021 AnalogIn s5(A3);
00022 AnalogIn s6(A2);
00023 AnalogIn s7(A1);
00024 AnalogIn s8(A0);
00025 
00026 
00027 
00028 
00029 //☆★☆★各種パラメータ調整箇所☆★☆★☆★
00030 
00031 #define DEFAULT_SPEED 0.3f   //機体の直進速度30％
00032 
00033 //フォトリフレクタのゲイン（外側に行くにつれ値を何倍させたいか調整する。）
00034 #define S_K1    1.0f    //float演算させる値には必ずfを付ける
00035 #define S_K2    2.0f    //2倍
00036 #define S_K3    4.0f    //4倍
00037 
00038 //ラインセンサ比例制御成分
00039 #define S_KP    0.3f    //ラインセンサによるモータ制御量
00040                         //大きいほど曲がりやすい
00041 ////////////////////////////////////////////////////////////////
00042 
00043 
00044 //使用変数の定義
00045 float S1_Data,S2_Data,S3_Data,S4_Data,S5_Data,S6_Data,S7_Data,S8_Data; 
00046 float All_Sensor_Data;
00047 float Sensor_P;
00048 float Motor_A_Pwm,Motor_B_Pwm;
00049 
00050 void timer_interrupt(){
00051         //各種センサ情報取得
00052        S1_Data=s1.read();
00053        S2_Data=s2.read();
00054        S3_Data=s3.read();
00055        S4_Data=s4.read();
00056        S5_Data=s5.read();
00057        S6_Data=s6.read();
00058        S7_Data=s7.read();
00059        S8_Data=s8.read();
00060               
00061         
00062        //センサ取得値の重ね合わせ（端のセンサほどモータ制御量を大きくする）
00063        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);
00064        Sensor_P=All_Sensor_Data*S_KP;//センサ比例成分の演算
00065 
00066        
00067        //モータ制御量の演算
00068        Motor_A_Pwm=DEFAULT_SPEED + Sensor_P;
00069        Motor_B_Pwm=DEFAULT_SPEED - Sensor_P;       
00070        //モータ出力は±1.0が上下限度なので限界値を設定する。
00071        if(Motor_A_Pwm> 1.0f)Motor_A_Pwm=1.0f;
00072        else if(Motor_A_Pwm< -1.0f)Motor_A_Pwm=-1.0f;
00073        if(Motor_B_Pwm> 1.0f)Motor_B_Pwm=1.0f;
00074        else if(Motor_B_Pwm< -1.0f)Motor_B_Pwm=-1.0f;       
00075        
00076        //最終的には符号を逆転して出力
00077        motor_a=-Motor_A_Pwm;
00078        motor_b=-Motor_B_Pwm;  
00079 }
00080 
00081 int main() { 
00082     timer.attach_us(&timer_interrupt,1000);//1ms単位     
00083     while(1) {
00084     }
00085 }