File content as of revision 2:db9d6e8ac3c1:

#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;             //汎用タイマー
Ticker sensget;             //センサー用タイマー

//プロトタイプ宣言
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;                    //ラインセンサ
int SensR[9],SensL[9];
double PGainCLB=0;          //Pゲイン調整変数
double IGainCLB=0;          //Iゲイン調整変数
double DGainCLB=0;//1.7;          //Dゲイン調整変数
double SensVal_I,SensVal_IBuf=0;
char turnFlg=0;
char Stime;
int CommSpeed;
//----------ボリューム値取得-----------------
int GetVol(void){
    int Val;
    Val = Volume.read_u16()>>12;
    return Val;
    }
//----------センサ値更新-----------------
void SensUp(void){
int i,j,SensBuf;
  for (i = 0; i < 9; i++) {
    for (j = 9; j > i; j--) {
      if (SensR[j-1] > SensR[j]) {
        SensBuf = SensR[j-1];
        SensR[j-1] = SensR[j];
        SensR[j] = SensBuf;
      }
      if (SensL[j-1] > SensL[j]) {
        SensBuf = SensL[j-1];
        SensL[j-1] = SensL[j];
        SensL[j] = SensBuf;
      }
    }
  }

    SensorR = SensR[5];
    SensorL = SensL[5];
    }
//----------センサー値の取得---------------
void sensGet(){
    if(Stime >= 9) {
        Stime=0;
        } 
    SensR[Stime] = LineR.read_u16();
    SensL[Stime] = LineL.read_u16();
    Stime++;
    }  
//------------モータ管理--------------------
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 >= 100) LMotorVal = 100;
        if(RMotorVal >= 100) RMotorVal = 100;
        PWM_L.pulsewidth_us(LMotorVal);    //左PWM  (0~1000)
        PWM_R.pulsewidth_us(RMotorVal);    //右PWM  (0~1000)
        //ledval = int(RMotorVal*0.016);
            }
        
        L_Dir = !MotorL_Rev;             //右モータ回転方向（H：CW）
        R_Dir = !MotorR_Rev;            //左モータ回転方向（L：FW）
                    }    
//------------ライントレース--------------------
void LineTrace(void){
    int SensVal,SensVal_D;
    double PGain=0.026,IGain=0.0002,DGain=1.6;
    if(SensorR <= 5000 && SensorL <= 5000) CommSpeed=0;
    SensVal = SensorR - SensorL;
    if(SensVal <= 0 && !turnFlg){//turnL
     turnFlg=1;
     SensVal_I = 0;
        }
     else if (SensVal >= 0 && turnFlg){//turnR
      turnFlg=0;
      SensVal_I = 0;
         }
    SensVal_I = SensVal_I + SensVal;
    SensVal_IBuf = SensVal_IBuf + SensVal;
    if(SensVal_I >= 100000000) SensVal_I = 100000000;
    if(SensVal_I <= (-100000000)) SensVal_I = (-100000000);
    SensVal_D = SensValBuf - SensVal;
    SensValBuf = SensVal;
    MotorR = int(((CommSpeed - ((SensVal * PGain)+(SensVal_I*IGain)-(SensVal_D*DGain))))/10);
    MotorL = int((CommSpeed + ((SensVal * PGain)+(SensVal_I*IGain)-(SensVal_D*DGain)))/10);
    ledval = int(SensVal_IBuf / (-100000));
    //MotorR = int(CommSpeed - (SensVal * PGainCLB));
    //MotorL = int(CommSpeed + (SensVal * PGainCLB));
    }
//-------------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(){
    MotorCtrl();
    led_out();
    SensUp();
    LineTrace();
    timer1++;
            }
            
//----------マイコン初期設定---------------
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);              //汎用タイマー割り込み
sensget.attach_us(&sensGet,100);            //センサー用タイマー割り込み 
//PWM周期設定
PWM_L.period(0.0001);
PWM_R.period(0.0001);
    }
    
//---------------メイン--------------------
int main() {
    init();
    int sensV;

    wait(1);
    while(SW_IN)
     {
        pc.printf("%5d  %5d \r\n",SensorL,SensorR);
        wait(0.1);
        }
    wait(1);
    MotorDA = 0;
    timer1 = 0;
    while(1) {
        if(!SW_IN)
         {
             //DGainCLB = DGainCLB + 0.1;
             pc.printf("%5d  %5d  %5d %5d %f %d \r\n",SensorL,SensorR,MotorL,MotorR,DGainCLB,turnFlg);
             //ledval++;
             wait(0.1);
             }   
             if(timer1 <= 1000)SensVal_IBuf = 0;
             if(SensVal_I <= (-10000) || SensVal_I >= 10000 )CommSpeed = 200;
              else CommSpeed=450;
        wait(0.01);

    }
}