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) {
    }
}