File content as of revision 1:f382b4244a89:

#include "mbed.h"

//シンボル定義
#define     FW      0       //前進
#define     BW      1       //後進

AnalogIn LineL(PA_0);       //アナログラインセンサ
AnalogIn LineR(PA_1);       //アナログラインセンサ
AnalogIn Volume(PA_4);      //ボリューム入力
DigitalOut led1(PB_7);      //LED_1
DigitalOut led2(PB_6);      //LED_2
DigitalOut led3(PB_5);      //LED_3
DigitalOut led4(PB_4);      //LED_4
DigitalOut L_Dir(PA_12);     //左モータ回転方向
DigitalOut R_Dir(PA_9);     //右モータ回転方向
DigitalOut MotorDA(PA_11);  //モーター出力EN
DigitalIn SW_IN(PB_3);      //スイッチ入力
PwmOut PWM_L(PA_8);         //左モータPWM
PwmOut PWM_R(PA_10);        //右モータPWM
//AQM0802A lcd(PB_7,PB_6);
Serial pc(USBTX, USBRX); // tx, rx


//割り込み定義
Ticker flipper;             //汎用タイマー

//プロトタイプ宣言
void    init(void);         //マイコン初期設定
void    led_out(void);      //LED出力
void    MotorCtrl(void);    //モータ管理
void    LineTrace(void);    //ライントレース
void    SensUp(void);       //センサー値更新
int     GetVol(void);       //ボリューム値取得

//グローバル変数の宣言
int timer1=0;                           //汎用タイマー
int ledval=1;                           //LED出力値
int MotorL_Rev=0,MotorR_Rev=0;          //モータの回転方向
int MotorL=0,MotorR=0;                  //モータPWMデューティ比
int ErrFlg=0;                           //エラー判定フラグ
int SensValBuf=0;                       //センサ値のバッファ
int SensorR,SensorL;                    //ラインセンサ
float VolumeVal;
//----------ボリューム値取得-----------------
int GetVol(void){
    int Val;
    Val = Volume.read_u16()>>7;
    return Val;
    }
//----------センサ値更新-----------------
void SensUp(void){
    SensorR = LineR.read_u16()>>4;
    SensorL = LineL.read_u16()>>4;
    }
//------------モータ管理--------------------
void MotorCtrl(void){
    int RMotorVal,LMotorVal;
    if(ErrFlg){                          //異常判定
        PWM_L.pulsewidth_us(0);         //左PWM出力0
        PWM_R.pulsewidth_us(0);         //右PWM出力0
                    }
    else{                               //通常時
        if(MotorL < 0 ){
            LMotorVal = MotorL * (-1);
            MotorL_Rev = BW;}
         else{
             LMotorVal = MotorL;
             MotorL_Rev = FW;}
         
        if(MotorR < 0 ){
            RMotorVal = MotorR * (-1);
            MotorR_Rev = BW;}
         else{
             RMotorVal = MotorR;
             MotorR_Rev = FW;}
        
        if(LMotorVal >= 1000) MotorL = 1000;
        if(RMotorVal >= 1000) MotorR = 1000;
        PWM_L.pulsewidth_us(LMotorVal);    //左PWM  (0~1000)
        PWM_R.pulsewidth_us(RMotorVal);    //右PWM  (0~1000)
            }
        
        L_Dir = MotorL_Rev;             //右モータ回転方向（H：CW）
        R_Dir = !MotorR_Rev;            //左モータ回転方向（L：FW）
                    }    
//------------ライントレース--------------------
void LineTrace(void){
    long SensVal,CommSpeed=0,sensdelta=0,SensValBuf=0,SensI;
    //float PGain=6.3,DGain=4;
    //float PGain=0.18,DGain=1.07;
    float PGain=0.75,DGain=0,IGain=0.1;
    //DGain=0;
    DGain=VolumeVal;
    //PGain=VolumeVal;
    //IGain=VolumeVal;
    //CommSpeed=100*VolumeVal;
    SensVal = SensorR - SensorL;
    sensdelta = SensVal - SensValBuf;
    SensI = SensI + SensVal;
    if(sensdelta < 0 ) sensdelta = sensdelta * (-1);
    MotorR = int(CommSpeed - (SensVal * PGain) - (sensdelta * DGain) - (SensI * IGain));
    //MotorL = int(1.19 * (CommSpeed + (SensVal * PGain) - (sensdelta * DGain)));
    MotorL = int(CommSpeed + (SensVal * PGain) - (sensdelta * DGain) + (SensI * IGain));
    SensValBuf = SensVal;
    }
//-------------LED出力------------------
void led_out(void){
    if(ledval & 0x01) led1 = 1;
        else led1 = 0;
    if(ledval & 0x02) led2 = 1;
        else led2 = 0;
    if(ledval & 0x04) led3 = 1;
        else led3 = 0;
    if(ledval & 0x08) led4 = 1;
        else led4 = 0;
                    }

//----------タイマー割り込み---------------
void flip(){
    timer1++;
    MotorCtrl();
    led_out();
    SensUp();
    //LineTrace();
            }
            
//----------マイコン初期設定---------------
void init(void){
//I/O設定
/*
RS1.mode(PullDown);                 //ロータリスイッチbit1
RS2.mode(PullDown);                 //ロータリスイッチbit2
RS3.mode(PullDown);                 //ロータリスイッチbit3
RS4.mode(PullDown);                 //ロータリスイッチbit4*/
SW_IN.mode(PullUp);             //スイッチ入力ピンプルアップ
MotorDA = 1;

//割り込み処理開始  
flipper.attach_us(&flip,1000);              //汎用タイマー割り込み
//PWM周期設定
PWM_L.period(0.001);
PWM_R.period(0.001);
    }
    
//---------------メイン--------------------
int main() {
    init();
    float tmpval;
    wait(1);
    while(SW_IN);
    MotorDA = 0;
    while(1) {
        //tmpval=GetVol();
        //VolumeVal=tmpval/100;
        if(!SW_IN) pc.printf("%5d  %5d  %5d %5d %f \r\n",SensorL,SensorR,MotorL,MotorR,VolumeVal);
        //ledval = sensV;
        MotorR  = 300;
        MotorL  = 300; 
        ledval ++;      
        wait(0.5);
        MotorR  = -300;
        MotorL  = -300; 
        ledval ++;      
        wait(0.5);

    }
}