main.cpp

00001 #include "mbed.h"
00002 #include "MultiSerial.h"
00003 
00004 #define KEYCODE 0xAA
00005 #define DATA_NUM 1 //DATA_NUM[byte]通信
00006 #define TIME_LIMIT 1.0//腕が閉まっても動けるのはTIME_LIMIT[s]まで
00007 #define ARM_REV 1.0
00008 #define TURN_REV 0.25
00009  
00010 uint8_t INdata[DATA_NUM]={0}, EXdata[DATA_NUM]={0};
00011 volatile uint8_t count=0;
00012  
00013 enum Logic{OFF=0, ON};//殆どはモーターのON，OFFに使う
00014 enum Arm{R=0, L, C};//右腕と左腕
00015 enum Clip{Open=0, Close};//開ける動作と閉める動作
00016 enum Revolution{CW=0, CCW};//正回転，逆回転(上体回転)
00017 enum Compass{N=0, E, W, S};//マシンの回転
00018  
00019 BusOut LED(LED1, LED2, LED3, LED4);//確認用
00020 BusIn mt(p19, p20, p17, p18);
00021 //DigitalIn Ninety[2]={p5, p6};//R, L
00022 DigitalIn OneEighty[2]={p5, p6};//R, L
00023 InterruptIn HandSW[3]={p7, p8, p9};//p14, p15, p16//R, L, C
00024 DigitalIn TurnSW[2]={p11, p12};//
00025 PwmOut HandSpeed[2]={p23, p24};
00026 PwmOut TurnSpeed(p22);//p22
00027 DigitalOut Hand[2][2]={{p28, p27}, {p26, p25}};//{p17, p18}, {p19, p20}
00028 //ex)Hand[R][Close]=ON;
00029 DigitalOut Turn[2]={p29, p30};//p29, p30
00030 //ex)Turn[CW]=ON;
00031 Serial pc(USBTX, USBRX);
00032 MultiSerial Xbee(p13, p14, read);
00033 Ticker flick;
00034 Timeout Touch;//腕で掴むときの安全対策
00035  
00036 void StopCatching(void);//literally
00037 void StopLeaving(void);//literally
00038 void StartCatching(void);//literally
00039 void StartLeaving(void);//literally
00040 void RightCatch(void);//右腕のリミットスイッチ
00041 void LeftCatch(void);//左腕のリミットスイッチ
00042 void CenterCatch(void);//中央のリミットスイッチ
00043 void illumination(void);//literally
00044 void SetUp(void);//literally
00045 bool GetBit(uint8_t n, uint8_t bit);//literally
00046 
00047 int main(void) {
00048     
00049     SetUp();
00050     
00051     while(1) {
00052         
00053         //INdata[0] = ~mt;
00054         
00055         if(GetBit(INdata[0], 2)==OFF) StopLeaving();//停止
00056         
00057         if((GetBit(INdata[0], 1)+GetBit(INdata[0], 2))==1){//同時押し防止
00058             if(GetBit(INdata[0], 1)==true) CenterCatch();//中央のリミットスイッチがうまく動作しなかった場合の保険
00059             if(GetBit(INdata[0], 2)==true){//腕を広げる動作はマニュアル操作
00060                 StopCatching();//停止
00061                 Touch.detach();//ワンショットタイマー停止
00062                 Hand[R][Open] = GetBit(INdata[0], 2)&&OneEighty[R];
00063                 Hand[L][Open] = GetBit(INdata[0], 2)&&OneEighty[L];
00064             }
00065         }
00066         
00067         if(((!HandSW[R])&&(!HandSW[L]))&&((Hand[R][Close]&&Hand[L][Close])==ON)){//どちらも押されたら停止
00068             StopCatching();//停止
00069             Touch.detach();//ワンショットタイマー停止
00070         }
00071         
00072         if(!OneEighty[R]) Hand[R][Open]=OFF;
00073         if(!OneEighty[L]) Hand[L][Open]=OFF;
00074         //if(!Ninety[R]) Hand[R][Close]=OFF;
00075         //if(!Ninety[L]) Hand[L][Close]=OFF;
00076         
00077         //if((!Ninety[R])&&(!Ninety[L])){//どちらも押されたら停止
00078         //    StopCatching();//停止
00079         //    Touch.detach();//ワンショットタイマー停止
00080         //}
00081         
00082         LED = ((0x01 * !HandSW[R]) | (0x02 * !HandSW[L]) | (0x04 * !HandSW[C]) | (0x08 * ((!OneEighty[R])&&(!OneEighty[L])) ));
00083         
00084         //上体回転
00085         
00086         if(GetBit(INdata[0], 3)==OFF) Turn[CW] = OFF;//停止
00087         if(GetBit(INdata[0], 4)==OFF) Turn[CCW] = OFF;//停止
00088         
00089         if((GetBit(INdata[0], 3)+GetBit(INdata[0], 4))==1){//同時押し防止
00090             
00091             Turn[CW] = GetBit(INdata[0], 3)&&TurnSW[N];//押されていて，かつリミットスイッチが押されていなかったら右回転
00092             Turn[CCW] = GetBit(INdata[0], 4)&&TurnSW[E];//押されていて，かつリミットスイッチが押されていなかったら左回転
00093             
00094         }
00095     }
00096 }
00097  
00098 void StopCatching(void){//停止
00099     Hand[R][Close]=Hand[L][Close]=OFF;
00100 }
00101 void StopLeaving(void){//停止
00102     Hand[R][Open]=Hand[L][Open]=OFF;
00103 }
00104 void StartCatching(void){//掴む（リミットスイッチが押されてなければ）
00105  
00106     if((!HandSW[R])&&(!HandSW[L])){
00107         Hand[R][Close] = Hand[L][Close] = OFF;
00108     }
00109     else{
00110         
00111         //Hand[R][Close]=Ninety[R];
00112         //Hand[L][Close]=Ninety[L];
00113         
00114         Hand[R][Close]=ON;
00115         Hand[L][Close]=ON;
00116             
00117     }
00118     
00119 }
00120 void StartLeaving(void){//離す（リミットスイッチが押されてなければ）
00121     Hand[R][Open]=OneEighty[R];
00122     Hand[L][Open]=OneEighty[L];
00123 }
00124 void RightCatch(void){//右腕のリミットスイッチ
00125     if((!HandSW[R])&&(!HandSW[L])){//どちらも押されたら停止
00126         StopCatching();//停止
00127         Touch.detach();//ワンショットタイマー停止
00128     }
00129 }
00130 void LeftCatch(void){//左腕のリミットスイッチ
00131     if((!HandSW[R])&&(!HandSW[L])){//どちらも押されたら停止
00132         StopCatching();//停止
00133         Touch.detach();//ワンショットタイマー停止
00134     }
00135 }
00136 void CenterCatch(void){//中央のリミットスイッチ
00137  
00138     if((!OneEighty[R])&&(!OneEighty[L])){
00139         Touch.attach(StopCatching, TIME_LIMIT);//TIME_LIMIT[s]で停止
00140         StartCatching();
00141     }
00142     
00143 }
00144 void illumination(void){//literally
00145     count++;
00146     LED = count;
00147 }
00148 void SetUp(void){//literally
00149     
00150     mt.mode(PullUp);
00151     
00152     HandSpeed[R] = HandSpeed[L] = ARM_REV;
00153     TurnSpeed = TURN_REV;
00154     
00155     //flick.attach(illumination, .5);
00156     
00157     HandSW[R].mode(PullUp);
00158     HandSW[L].mode(PullUp);
00159     HandSW[C].mode(PullUp);
00160     HandSW[R].fall(RightCatch);
00161     HandSW[L].fall(LeftCatch);
00162     HandSW[C].fall(CenterCatch);
00163     
00164     OneEighty[R].mode(PullUp);
00165     OneEighty[L].mode(PullUp);
00166     //Ninety[R].mode(PullUp);
00167     //Ninety[L].mode(PullUp);
00168     
00169     TurnSW[N].mode(PullUp);
00170     TurnSW[E].mode(PullUp);
00171     
00172     Xbee.read_data(INdata, KEYCODE);
00173 }
00174 bool GetBit(uint8_t n, uint8_t bit){//n=8[bit]data, bit = 1-8(8bitのデータから指定bitの数値を読み取る)
00175     return (n>>(bit-1))%2;
00176 }