Diff: main.cpp

Revision:

0:21afd0549d07

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sat Apr 28 06:26:41 2018 +0000
@@ -0,0 +1,145 @@
+#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;                    //ラインセンサ
+//----------ボリューム値取得-----------------
+int GetVol(void){
+    int Val;
+    Val = Volume.read_u16()>>12;
+    return Val;
+    }
+//----------センサ値更新-----------------
+void SensUp(void){
+    SensorR = LineR.read_u16()>>8;
+    SensorL = LineL.read_u16()>>8;
+    }
+//------------モータ管理--------------------
+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){
+    int SensVal,CommSpeed=100,PGain=1;
+    
+    SensVal = SensorR - SensorL;
+    MotorR = int(CommSpeed - (SensVal * PGain));
+    MotorL = int(CommSpeed + (SensVal * PGain));
+    }
+//-------------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();
+    int sensV;
+    wait(1);
+    while(SW_IN);
+    MotorDA = 0;
+    while(1) {
+        if(!SW_IN) pc.printf("%5d  %5d  %5d %5d \r\n",SensorL,SensorR,MotorL,MotorR);
+        ledval = sensV;      
+        wait(0.02);
+
+    }
+}
\ No newline at end of file