File content as of revision 5:2f1098259948:

//モータドライバTB6612動作検証用のサンプルプログラム
//TB6612クラスを使用して、モータA,モータBのオブジェクトを生成する。
//生成時のピン割当はマイコンピン割当通りに配置すること。

//機体がきれいにライントレースするように各種パラメータ調整を行いこと。


#include "mbed.h"
#include "TB6612.h"
#include "AQM0802.h"
#include "mpu6050.h"
AQM0802 lcd(D4,D5);
MPU6050 mpu(D4, D5);
TB6612      motor_a(D2,D7,D6);  //モータA制御用(pwma,ain1,ain2)
TB6612      motor_b(D10,D8,D9);  //モータB制御用(pwmb,bin1,bin2)

Serial      pc(USBTX,USBRX);    //USBシリアル通信用

AnalogIn s1(A1);
AnalogIn s2(D3);
AnalogIn s3(A6);
AnalogIn s4(A5);
AnalogIn s5(A4);
AnalogIn s6(A3);
AnalogIn s7(A2);
AnalogIn s8(A0);




//☆★☆★各種パラメータ調整箇所☆★☆★☆★

#define DEFAULT_SPEED 0.15f   //機体の直進速度30％

//フォトリフレクタのゲイン（外側に行くにつれ値を何倍させたいか調整する。
#define S_K1    1.4f    //float演算させる値には必ずfを付ける
#define S_K2    2.5f    //2倍
#define S_K3    3.7f    //4倍

//ラインセンサ比例制御成分
#define S_KP    0.07f    //ラインセンサによるモータ制御量
                        //大きいほど曲がりやすい

//ジャイロセンサのゲイン
//#define G_KP    0.75f
#define G_KP    0.0f    //ジャイロ未使用の場合は0.0(制御量無し)にしておく

////////////////////////////////////////////////////////////////






//使用変数の定義
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;
char  Str_S[4],Str_G[4];
double Gx,Gy,Gz;    //Z角速度情報を格納
float Gz_F;         //Ｚ軸データint型変換格納
float Gyro_P;       //ジャイロセンサP成分

int main() {
    lcd.init();         //LCD初期化
    lcd.locate(0,0);    //桁、行    
    mpu.setMaxScale(MAX_ACC_8G, MAX_GYRO_1000degpersec);//[1000deg/sec]を測定上限とする。
    
         
    while(1) {
       wait_ms(10);
       //各種センサ情報取得
       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();

        //★☆角速度情報の取得☆★
        mpu.readGyroscope(Gx, Gy, Gz);//関数仕様上３軸すべて角速度取得する。
        Gz_F=(float)Gz/1000;   //doubleは大きすぎるのでfloat型へ変換  
        Gyro_P=Gz_F*G_KP;                  
        
       //センサ取得値の重ね合わせ（端のセンサほどモータ制御量を大きくする）
       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;//センサ比例成分の演算

        //LCD1行目にセンサ合計値の表示
       lcd.locate(0,0);//桁、行
       lcd.print("        ");//表示クリア
       lcd.locate(0,0);
       lcd.print("S:");
       sprintf(Str_S,"%+1.2f",Sensor_P);//センサ比例成分を文字列変換       
       lcd.print(Str_S);
 
        //★☆LCDへの角速度表示☆★        
        lcd.locate(0,1);//桁、行
        lcd.print("        ");//表示クリア
        lcd.locate(0,1);
        lcd.print("G:");
        sprintf(Str_G,"%+1.2f",Gz_F);
        lcd.print(Str_G);  
       
       //モータ制御量の演算
       Motor_A_Pwm=DEFAULT_SPEED + Sensor_P - Gyro_P;
       Motor_B_Pwm=DEFAULT_SPEED - Sensor_P + Gyro_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;

    }
}