main.cpp

00001 #include "mbed.h"
00002 
00003 //シンボル定義
00004 #define     FW      0       //前進
00005 #define     BW      1       //後進
00006 
00007 AnalogIn LineL(PA_0);       //アナログラインセンサ
00008 AnalogIn LineR(PA_1);       //アナログラインセンサ
00009 AnalogIn Volume(PA_4);      //ボリューム入力
00010 DigitalOut led1(PB_7);      //LED_1
00011 DigitalOut led2(PB_6);      //LED_2
00012 DigitalOut led3(PB_5);      //LED_3
00013 DigitalOut led4(PB_4);      //LED_4
00014 DigitalOut L_Dir(PA_12);     //左モータ回転方向
00015 DigitalOut R_Dir(PA_9);     //右モータ回転方向
00016 DigitalOut MotorDA(PA_11);  //モーター出力EN
00017 DigitalIn SW_IN(PB_3);      //スイッチ入力
00018 PwmOut PWM_L(PA_8);         //左モータPWM
00019 PwmOut PWM_R(PA_10);        //右モータPWM
00020 //AQM0802A lcd(PB_7,PB_6);
00021 Serial pc(USBTX, USBRX); // tx, rx
00022 
00023 
00024 //割り込み定義
00025 Ticker flipper;             //汎用タイマー
00026 Ticker sensget;             //センサー用タイマー
00027 
00028 //プロトタイプ宣言
00029 void    init(void);         //マイコン初期設定
00030 void    led_out(void);      //LED出力
00031 void    MotorCtrl(void);    //モータ管理
00032 void    LineTrace(void);    //ライントレース
00033 void    SensUp(void);       //センサー値更新
00034 int     GetVol(void);       //ボリューム値取得
00035 
00036 
00037 //グローバル変数の宣言
00038 int timer1=0;                           //汎用タイマー
00039 int ledval=1;                           //LED出力値
00040 int MotorL_Rev=0,MotorR_Rev=0;          //モータの回転方向
00041 int MotorL=0,MotorR=0;                  //モータPWMデューティ比
00042 int ErrFlg=0;                           //エラー判定フラグ
00043 int SensValBuf=0;                       //センサ値のバッファ
00044 int SensorR,SensorL;                    //ラインセンサ
00045 int SensR[9],SensL[9];
00046 double PGainCLB=0;          //Pゲイン調整変数
00047 double IGainCLB=0;          //Iゲイン調整変数
00048 double DGainCLB=1.7;          //Dゲイン調整変数
00049 double SensVal_I,SensVal_IBuf=0;
00050 char turnFlg=0;
00051 char Stime;
00052 int CommSpeed;
00053 //----------ボリューム値取得-----------------
00054 int GetVol(void){
00055     int Val;
00056     Val = Volume.read_u16()>>12;
00057     return Val;
00058     }
00059 //----------センサ値更新-----------------
00060 void SensUp(void){
00061 int i,j,SensBuf;
00062   for (i = 0; i < 9; i++) {
00063     for (j = 9; j > i; j--) {
00064       if (SensR[j-1] > SensR[j]) {
00065         SensBuf = SensR[j-1];
00066         SensR[j-1] = SensR[j];
00067         SensR[j] = SensBuf;
00068       }
00069       if (SensL[j-1] > SensL[j]) {
00070         SensBuf = SensL[j-1];
00071         SensL[j-1] = SensL[j];
00072         SensL[j] = SensBuf;
00073       }
00074     }
00075   }
00076 
00077     SensorR = SensR[5];
00078     SensorL = SensL[5];
00079     }
00080 //----------センサー値の取得---------------
00081 void sensGet(){
00082     if(Stime >= 9) {
00083         Stime=0;
00084         } 
00085     SensR[Stime] = LineR.read_u16();
00086     SensL[Stime] = LineL.read_u16();
00087     Stime++;
00088     }  
00089 //------------モータ管理--------------------
00090 void MotorCtrl(void){
00091     int RMotorVal,LMotorVal;
00092     if(ErrFlg){                          //異常判定
00093         PWM_L.pulsewidth_us(0);         //左PWM出力0
00094         PWM_R.pulsewidth_us(0);         //右PWM出力0
00095                     }
00096     else{                               //通常時
00097         if(MotorL < 0 ){
00098             LMotorVal = MotorL * (-1);
00099             MotorL_Rev = BW;}
00100          else{
00101              LMotorVal = MotorL;
00102              MotorL_Rev = FW;}
00103          
00104         if(MotorR < 0 ){
00105             RMotorVal = MotorR * (-1);
00106             MotorR_Rev = BW;}
00107          else{
00108              RMotorVal = MotorR;
00109              MotorR_Rev = FW;}
00110         
00111         if(LMotorVal >= 100) LMotorVal = 100;
00112         if(RMotorVal >= 100) RMotorVal = 100;
00113         PWM_L.pulsewidth_us(LMotorVal);    //左PWM  (0~1000)
00114         PWM_R.pulsewidth_us(RMotorVal);    //右PWM  (0~1000)
00115         //ledval = int(RMotorVal*0.016);
00116             }
00117         
00118         L_Dir = MotorL_Rev;             //右モータ回転方向（H：CW）
00119         R_Dir = !MotorR_Rev;            //左モータ回転方向（L：FW）
00120                     }    
00121 //------------ライントレース--------------------
00122 void LineTrace(void){
00123     int SensVal,SensVal_D;
00124     double PGain=0.026,IGain=0.0002,DGain=0.7;
00125     if(SensorR <= 5000 && SensorL <= 5000) CommSpeed=0;
00126     SensVal = SensorR - SensorL;
00127     if(SensVal <= 0 && !turnFlg){//turnL
00128      turnFlg=1;
00129      SensVal_I = 0;
00130         }
00131      else if (SensVal >= 0 && turnFlg){//turnR
00132       turnFlg=0;
00133       SensVal_I = 0;
00134          }
00135     SensVal_I = SensVal_I + SensVal;
00136     SensVal_IBuf = SensVal_IBuf + SensVal;
00137     if(SensVal_I >= 100000000) SensVal_I = 100000000;
00138     if(SensVal_I <= (-100000000)) SensVal_I = (-100000000);
00139     SensVal_D = SensValBuf - SensVal;
00140     SensValBuf = SensVal;
00141     MotorR = int((CommSpeed - ((SensVal * PGain)+(SensVal_I*IGain)-(SensVal_D*DGainCLB))))/10;
00142     MotorL = int(CommSpeed + ((SensVal * PGain)+(SensVal_I*IGain)-(SensVal_D*DGainCLB)))/10;
00143     ledval = int(SensVal_IBuf / (-100000));
00144     //MotorR = int(CommSpeed - (SensVal * PGainCLB));
00145     //MotorL = int(CommSpeed + (SensVal * PGainCLB));
00146     }
00147 //-------------LED出力------------------
00148 void led_out(void){
00149     if(ledval & 0x01) led1 = 1;
00150         else led1 = 0;
00151     if(ledval & 0x02) led2 = 1;
00152         else led2 = 0;
00153     if(ledval & 0x04) led3 = 1;
00154         else led3 = 0;
00155     if(ledval & 0x08) led4 = 1;
00156         else led4 = 0;
00157                     }
00158 
00159 //----------タイマー割り込み---------------
00160 void flip(){
00161     MotorCtrl();
00162     led_out();
00163     SensUp();
00164     LineTrace();
00165     timer1++;
00166             }
00167             
00168 //----------マイコン初期設定---------------
00169 void init(void){
00170 //I/O設定
00171 /*
00172 RS1.mode(PullDown);                 //ロータリスイッチbit1
00173 RS2.mode(PullDown);                 //ロータリスイッチbit2
00174 RS3.mode(PullDown);                 //ロータリスイッチbit3
00175 RS4.mode(PullDown);                 //ロータリスイッチbit4*/
00176 SW_IN.mode(PullUp);             //スイッチ入力ピンプルアップ
00177 MotorDA = 1;
00178 
00179 //割り込み処理開始  
00180 flipper.attach_us(&flip,1000);              //汎用タイマー割り込み
00181 sensget.attach_us(&sensGet,100);            //センサー用タイマー割り込み 
00182 //PWM周期設定
00183 PWM_L.period(0.0001);
00184 PWM_R.period(0.0001);
00185     }
00186     
00187 //---------------メイン--------------------
00188 int main() {
00189     init();
00190     int sensV;
00191 
00192     wait(1);
00193     while(SW_IN)
00194      {
00195         pc.printf("%5d  %5d \r\n",SensorL,SensorR);
00196         wait(0.1);
00197         }
00198     wait(1);
00199     MotorDA = 0;
00200     timer1 = 0;
00201     while(1) {
00202         if(!SW_IN)
00203          {
00204              DGainCLB = DGainCLB + 0.1;
00205              pc.printf("%5d  %5d  %5d %5d %f %d \r\n",SensorL,SensorR,MotorL,MotorR,DGainCLB,turnFlg);
00206              //ledval++;
00207              }   
00208              if(timer1 <= 1000)SensVal_IBuf = 0;
00209              if(SensVal_I <= (-10000) || SensVal_I >= 10000 )CommSpeed = 200;
00210               else CommSpeed=490;
00211         wait(0.01);
00212 
00213     }
00214 }