main.cpp

00001 /**
00002   ******************************************************************************
00003   * @file    main.cpp 
00004   * @author  KTE makabe
00005   * @version V1.00
00006   * @date    2016/05/26
00007   * @brief   UC MIRAI program body
00008   ******************************************************************************
00009   *
00010   ******************************************************************************  
00011   */ 
00012 
00013 /* Includes ------------------------------------------------------------------*/
00014 #include "mbed.h"
00015 
00016 #define VALID_PWM // PWM有効
00017 
00018 /* Private typedef -----------------------------------------------------------*/
00019 /*状態*/
00020 typedef enum {
00021     Idle = 0,
00022     MoveL,
00023     MoveR,
00024     Origin,
00025     Error,
00026 } State_Enum;
00027 
00028 /* Private define ------------------------------------------------------------*/
00029 #define STOP()     {FWD=0; RVS=0;}
00030 #define FORWARD()  {FWD=MVR; RVS=0;}
00031 #define REVERSE()  {FWD=0; RVS=MVR;}
00032 #define BRAKE()    {FWD=1; RVS=1;}
00033 
00034 #define IS_TIMEOUT()   (g.cnt[0] <= 0)
00035 #define IS_OVER()      (OL == 1) /*過負荷*/
00036 #define IS_ERROR()     (IS_TIMEOUT() || IS_OVER())
00037 
00038 /* Private macro -------------------------------------------------------------*/
00039 /* Private variables ---------------------------------------------------------*/
00040 /*ピン定義*/
00041 DigitalOut db0(dp9);
00042 DigitalOut db1(dp10);
00043 DigitalOut db2(dp11);
00044 
00045 AnalogIn MVR(dp13); // モータ速度Input
00046 DigitalIn OL(dp14); // 過負荷検知(H)
00047 PwmOut FWD(dp1/*dp15*/); // 正転
00048 PwmOut RVS(dp2/*dp16*/); // 逆転
00049 DigitalIn LMT_L(dp4/*dp24*/); // リミットL(L)
00050 DigitalIn LMT_R(dp25); // リミットR(L)
00051 DigitalOut RED(dp26); // 赤LED(H)
00052 //DigitalIn TRIG(dp27); // トリガSW(H)
00053 InterruptIn TRIG(dp27);
00054 DigitalOut GRN(dp28); // 緑LED(H)
00055 
00056 Ticker tick;
00057 
00058 struct {
00059     int trig;
00060     State_Enum state;
00061     State_Enum lastState;
00062     int lastTrig; // 前回のトリガ値
00063     int cnt[4];
00064 } g;
00065 
00066 /* Private function prototypes -----------------------------------------------*/
00067 /* Private functions ---------------------------------------------------------*/
00068 void DBG(uint8_t n)
00069 {
00070     db0 = (n & 0x01);
00071     db1 = (n & 0x02) >> 1;
00072     db2 = (n & 0x04) >> 2;
00073 }
00074 void GoState(State_Enum state)
00075 {
00076     g.lastState = g.state;
00077     g.state = state;
00078     
00079     if(state == Error){
00080         g.cnt[0] = 0;
00081         g.cnt[1] = 0;
00082         g.cnt[2] = 25; // 250msec RED timer
00083         GRN = 0;
00084     }
00085     if(state == MoveL || state == MoveR || state == Origin){
00086         g.cnt[0] = 500; // 3sec timer start
00087         g.cnt[1] = 25; // 250msec GRN timer
00088         g.cnt[2] = 0;
00089     }
00090     if(state == Idle){
00091         g.cnt[0] = 0;
00092         g.cnt[1] = 0;
00093         g.cnt[2] = 0;
00094         GRN = 1;
00095         RED = 0;
00096     }
00097 }
00098 /*10msec handler*/
00099 void ticker_handler(void)
00100 {
00101     // TIMEOUT
00102     if(g.cnt[0] > 0 && --g.cnt[0] <= 0) {
00103     }
00104     // GREEN LED
00105     if(g.cnt[1] > 0 && --g.cnt[1] <= 0) {
00106         GRN = !GRN; // モータ動作中
00107         g.cnt[1] = 25;
00108     }
00109     // RED LED
00110     if(g.cnt[2] > 0 && --g.cnt[2] <= 0) {
00111         RED = !RED; // エラー点滅
00112         g.cnt[2] = 25;
00113     }
00114 }
00115 /*TRIG Interrupt*/
00116 void trig_handler(void)
00117 {
00118     g.trig = 1;
00119 }
00120 /* Public functions ----------------------------------------------------------*/
00121 /*----------------------------------------------------------------------------
00122 UC MIRAI PROGRAM             by makabe
00123 仕様
00124 ・トリガSWにてモーターを動かす。
00125 ・リミット到達したらモーターを停める。
00126 ・モーターは片方向動作。つまり[L->R]の次は[R->L]。これの繰り返し。
00127 ・動作異常時は赤LED点滅。（過負荷異常、タイムアウト）
00128 ・通常モーターはLかRどちらかに位置する。つまり一方のリミットセンサーが働いている状態。
00129 ・したがってモータが中途半端な位置にあった場合は原点復帰させる必要あり。
00130 -----------------------------------------------------------------------------*/
00131 /*----------------------------------------------------------------------------
00132 LED仕様
00133 ・RED消灯：電源OFF
00134 ・RED点灯：
00135 ・RED点滅：エラー時
00136 ・GRN消灯：電源OFF
00137 ・GRN点灯：正常動作時
00138 ・GRN点滅：モータ移動時
00139 -----------------------------------------------------------------------------*/
00140 /*int main() {
00141     //DBG(0);
00142     while(1) {
00143         RED = LMT_R;
00144         GRN = LMT_L;
00145     }
00146 }*/
00147 #if 1
00148 int main() 
00149 {
00150     memset(&g, 0, sizeof(g));
00151     tick.attach(&ticker_handler, 0.01);
00152     TRIG.mode(PullDown);
00153     TRIG.rise(&trig_handler);
00154     LMT_L.mode(PullDown);
00155     LMT_R.mode(PullDown);
00156     OL.mode(PullDown);
00157     FWD.period(0.001); // PWM period 1ms (1kHz)
00158     RVS.period(0.001);
00159     RED = 0;
00160     GRN = 1;
00161     STOP();
00162     
00163     g.lastTrig = 1;
00164     GoState(Idle);
00165     while(1) {
00166         switch(g.state) {
00167         case Idle:
00168 DBG(0);
00169             // トリガがあればモータを動かす
00170             // ただし、「トリガONで電源ON」の場合は動かしたくない。
00171             if(g.trig == 1/*g.lastTrig == 0 && TRIG == 1*/){
00172                 g.trig = 0;
00173                 if(LMT_L == 1 && LMT_R == 0) {
00174                     // モーター右にあれば左へ動かす
00175 L001:
00176                     FORWARD();
00177                     GoState(MoveL);
00178                 } else if(LMT_L == 0 && LMT_R == 1) {
00179                     // モーター左にあれば右へ動かす
00180                     REVERSE();
00181                     GoState(MoveR);
00182                 } else if(LMT_L == 1 && LMT_R == 1) {
00183                     // モータ中途半端な位置にある場合、まず左原点復帰。その後右移動
00184                     REVERSE();
00185                     GoState(Origin);
00186                 } else {
00187                     GoState(Error);
00188                 }
00189             }
00190             break;
00191         case MoveL:
00192 DBG(6);
00193             // 左移動中リミット到達したら停める
00194             if(LMT_L == 0){
00195                 BRAKE();
00196                 wait(0.5);
00197                 STOP();
00198                 GoState(Idle);
00199             } else if(IS_ERROR()){
00200                 STOP();
00201                 GoState(Error);
00202             }
00203             break;
00204         case MoveR:
00205 DBG(5);/*ok*/
00206             // 右移動中リミット到達したら停める
00207             if(LMT_R == 0){
00208                 BRAKE();
00209                 wait(0.5);
00210                 STOP();
00211                 GoState(Idle);
00212             } else if(IS_ERROR()){
00213                 STOP();
00214                 GoState(Error);
00215             }
00216             break;
00217         case Origin:
00218 DBG(4);
00219             // 原点復帰終えたら直ちに右移動
00220             if(LMT_R == 0){
00221                 BRAKE();
00222                 wait(0.5);
00223                 STOP();
00224                 goto L001;
00225             } else if(IS_ERROR()){
00226                 BRAKE();
00227                 wait(0.5);
00228                 STOP();
00229                 GoState(Error);
00230             }
00231             break;
00232         case Error:
00233 DBG(3);
00234             // エラーに遷移するともう戻れない。赤LED点滅
00235             break;    
00236         default:
00237             break;
00238         }//end_switch        
00239         //g.lastTrig = TRIG;
00240     }//end_while
00241 }
00242 #endif