Diff: main.cpp

Revision:

11:1ee158b83f3f

Parent:

10:557c74f2bdee

--- a/main.cpp	Thu Oct 16 11:25:17 2014 +0000
+++ b/main.cpp	Thu Oct 16 23:25:51 2014 +0000
@@ -4,11 +4,11 @@
  
 #define DATA_NUM 1 //DATA_NUM[byte]通信
 #define KEYCODE 0xAA
-
+ 
 #define TIME_LIMIT 1.25//腕が閉まっても動けるのはTIME_LIMIT[s]まで
 #define ARM_REV 1.0//腕のPWM
 #define ARM_LIMIT_REV 0.5//
-
+ 
 #define TURN_REV 0.5//回転の初期PWM値
 #define FIX_REV 0.2
  
@@ -17,7 +17,7 @@
 bool Interrupt=0;
 bool IO[2]={0};
 bool FIX=0;
-
+ 
 enum Logic{OFF=0, ON};//殆どはモーターのON，OFFに使う
 enum Arm{R=0, L, C};//右腕と左腕
 enum Clip{Open=0, Close};//開ける動作と閉める動作
@@ -26,10 +26,9 @@
  
 BusOut LED(LED1, LED2, LED3, LED4);//確認用
 BusIn mt(p5, p6, p7, p8);//確認用
-//DigitalOut servo_mbed[2] = {p9, p10};
-MultiSerial servo_mbed(p9, p10);
+//MultiSerial servo_mbed(p9, p10);
 MultiSerial leg_mbed(p13,p14);
-
+ 
 DigitalIn OneEighty[2]={p18, p19};//R, L
 Pswitch HandSW[3]={p15, p16, p17};//R, L, C
 DigitalIn TurnSW[2]={p11, p12};//
@@ -41,7 +40,7 @@
 //ex)Turn[CW]=ON;
 Serial pc(USBTX, USBRX);
 Timeout Touch;//腕で掴むときの安全対策
-
+ 
 void StopCatching(void);//literally
 void StopLeaving(void);//literally
 void StartCatching(void);//literally
@@ -49,7 +48,6 @@
 void RightCatch(void);//右腕のリミットスイッチ
 void LeftCatch(void);//左腕のリミットスイッチ
 void CenterCatch(void);//中央のリミットスイッチ
-void FixToNinety(void);
 void display_LED(int kind);//start display LED
 uint8_t LinkBit(bool eight, bool seven, bool six, bool five, bool fore, bool three, bool two, bool one);
 bool GetBit(uint8_t n, uint8_t bit);
@@ -58,39 +56,22 @@
  
 int main(void) {
     
-    //leg_mbed.start_read();
-    //leg_mbed.read_data(data,KEYCODE);
-
-    servo_mbed.start_write();
-    servo_mbed.write_data(INdata,KEYCODE);
-
-    //display_LED(1);
+    SetUp();
+    
+    leg_mbed.start_read();
+    leg_mbed.read_data(data,KEYCODE);
+ 
+    //servo_mbed.start_write();
+    //servo_mbed.write_data(INdata,KEYCODE);
+ 
+    display_LED(1);
         
-    //leg_mbed.check_rx_wait();
-    
-    //wait(0.5);
-    
-    //MultiSerial leg_mbed(p13,p14);
-    
-    SetUp();
+    leg_mbed.check_rx_wait();
     
     while(1) {
         
-        
-        //NVIC_DisableIRQ(UART0_IRQn);//通信割り込み禁止
-        //NVIC_DisableIRQ(UART1_IRQn);//通信割り込み禁止
-        //NVIC_DisableIRQ(UART2_IRQn);//通信割り込み禁止
-        //NVIC_DisableIRQ(UART3_IRQn);//通信割り込み禁止
-        //leg_mbed.read_data(data,KEYCODE);
-        
-        //INdata[0] = data[0];
-        INdata[0] = ~mt;
-        
-        //if(!TurnSW[N]) IO[0]=ON;
-        //if(!TurnSW[E]) IO[1]=ON;
-        //servo_mbed[0] = IO[0]&&IO[1];
-        //servo_mbed[1] = IO[0]&&IO[1];
-        //servo_mbed.putc(IO[0]&&IO[1]);
+        INdata[0] = data[0];
+        //INdata[0] = ~mt;
         
         LED = INdata[0];
         
@@ -100,7 +81,7 @@
         
         if(GetBit(INdata[0], 2)==OFF) StopLeaving();//停止
         
-        if(GetBit(INdata[0], 1)&&GetBit(INdata[0], 2)) StopCatching(), StopLeaving(), Touch.detach(), Interrupt=OFF;
+        //if(GetBit(INdata[0], 1)&&GetBit(INdata[0], 2)) StopCatching(), StopLeaving(), Touch.detach(), Interrupt=OFF;
         
         if((GetBit(INdata[0], 1)+GetBit(INdata[0], 2))==1){//同時押し防止
             if(GetBit(INdata[0], 1)==true) CenterCatch();//中央のリミットスイッチがうまく動作しなかった場合の保険
@@ -132,22 +113,15 @@
             LeftCatch();
         }
         if(HandSW[C].count() > 0){
-            if(GetBit(INdata[0], 5)) CenterCatch();
+            CenterCatch();
         }
         
-        //LED = ((0x01 * !HandSW[R]) | (0x02 * !HandSW[L]) | (0x04 * !HandSW[C]) | (0x08 * ((!OneEighty[R])&&(!OneEighty[L])) ));
-        
         //上体回転
         
-        //if(FIX&&(!((GetBit(INdata[0], 3)+GetBit(INdata[0], 4))==1))){
-        //     FixToNinety();
-        //     continue;
-        //}
-        
         if(GetBit(INdata[0], 3)==OFF) Turn[CW] = OFF;//停止
         if(GetBit(INdata[0], 4)==OFF) Turn[CCW] = OFF;//停止
         
-        if(GetBit(INdata[0], 3)&&GetBit(INdata[0], 4)) Turn[CW]=Turn[CCW]=OFF;
+        //if(GetBit(INdata[0], 3)&&GetBit(INdata[0], 4)) Turn[CW]=Turn[CCW]=OFF;
         
         if((GetBit(INdata[0], 3)+GetBit(INdata[0], 4))==1){//同時押し防止
         
@@ -229,31 +203,19 @@
         StartCatching();
     }
 }
-void FixToNinety(void){
-    
-    if((FIX)&&(TurnSpeed != FIX_REV)) TurnSpeed = FIX_REV;
-    
-    if((!TurnSW[N])&&(TurnSW[E])) Turn[CCW]=ON;
-    
-    if((TurnSW[N])&&(!TurnSW[E])) Turn[CW]=ON;
-    
-    if((TurnSW[N])&&(TurnSW[E])) Turn[CW]=Turn[CCW]=OFF, FIX=OFF;
-    
-    if((!FIX)&&(TurnSpeed != TURN_REV)) TurnSpeed = TURN_REV;
-    
-}
+
 void display_LED(int kind){/*start display LED*/
-
+ 
     switch(kind) {
-
+ 
         case 0: //err check
             LED = 0xA;
             wait(0.5);
             LED = 0x5;
             wait(0.5);
-
+ 
             break;
-
+ 
         case 1:
             //
             LED = 0xF;
@@ -276,18 +238,10 @@
     HandSpeed[R] = HandSpeed[L] = ARM_REV;
     TurnSpeed = TURN_REV;
     
-    //HandSW[R].mode(PullUp);
-    //HandSW[L].mode(PullUp);
-    //HandSW[C].mode(PullUp);
-    //HandSW[R].fall(RightCatch);
-    //HandSW[L].fall(LeftCatch);
-    //HandSW[C].fall(CenterCatch);
-    
     OneEighty[R].mode(PullUp);
     OneEighty[L].mode(PullUp);
     
     TurnSW[N].mode(PullUp);
     TurnSW[E].mode(PullUp);
     
-}
-   
\ No newline at end of file
+}
\ No newline at end of file