Diff: main.cpp

Revision:

3:d4eff9dace7d

Parent:

1:b43b0cbff722

Child:

4:541fdc451ab3

--- a/main.cpp	Sat Jun 28 09:57:50 2014 +0000
+++ b/main.cpp	Mon Jul 14 10:03:17 2014 +0000
@@ -1,6 +1,8 @@
 #include "mbed.h"
 #include "main.h"//ピン位置，main関数までの初期動作の詳細，変数，関数等
 
+BusIn mt(p6, p7, p8, p9, p10, p11);
+
 extern bool GetBit(void);
 extern DigitalOut LED[4];
 //上体回転関連
@@ -8,7 +10,6 @@
 extern DigitalIn TurnSW;
 extern PwmOut TurnSpeed;
 //パワーウィンドウ(腕)関連
-extern bool Pushed[2];
 extern DigitalOut Hand[2][2];
 extern InterruptIn HandSW[3];
 extern Timeout Touch;
@@ -19,29 +20,20 @@
 extern volatile uint8_t INdata[DATA_NUM], EXdata[DATA_NUM];
 extern void TX(void);
 extern void RX(void);
+
+void SetUp(void);
+
 int main(void) {
-    //簡単そうなコードの説明は省く
-    BusIn mt(p6, p7, p8, p9, p10, p11);//調整用スイッチ
-    mt.mode(PullUp);
-    //上体回転関連
-    TurnSpeed = TurnRev;//上体回転のPWM
-    TurnSW.mode(PullUp);
-    //パワーウィンドウ(腕)関連
-    HandSW[R].mode(PullUp);
-    HandSW[L].mode(PullUp);
-    HandSW[C].mode(PullUp);
-    HandSW[R].rise(RightCatch);//立ち上がり
-    HandSW[L].rise(LeftCatch);//立ち上がり
-    HandSW[C].rise(CenterCatch);//立ち上がり
-    //通信関連
-    Xbee.attach(&RX, Serial::RxIrq);
-    Xbee.attach(&TX, Serial::TxIrq);
-    Xbee.putc(1);//送信開始
-    i = Xbee.getc();//受信開始
+    
+    SetUp();
+    
     while(1) {
         //腕のプログラム
+        
         INdata[1]= ~mt;//調整用スイッチ
-        StopLeaving();//停止
+        
+        if(GetBit(INdata[1], 2)==OFF) StopLeaving();//停止
+        
         if((GetBit(INdata[1], 1)+GetBit(INdata[1], 2))==1){//同時押し防止
             if(GetBit(INdata[1], 1)==true) CenterCatch();//中央のリミットスイッチがうまく動作しなかった場合の保険
             if(GetBit(INdata[1], 2)==true){//腕を広げる動作はマニュアル操作
@@ -50,16 +42,48 @@
                 Hand[R][Open] = Hand[L][Open] = GetBit(INdata[1], 2);
             }
         }
+        
+        if(((!HandSW[R])&&(!HandSW[L]))&&((Hand[R][Close]&&Hand[L][Close])==ON)){//どちらも押されたら停止
+            StopCatching();//停止
+            Touch.detach();//ワンショットタイマー停止
+        }
         //上体回転
+        
         INdata[3] = ~mt;//調整用スイッチ
+        
         Turn[CW] = Turn[CCW] = OFF;//停止
+        
         if((GetBit(INdata[3], 1)+GetBit(INdata[3], 2))==1){//同時押し防止
             if(GetBit(INdata[3], 1)&&(TurnSW)){
                 Turn[CCW] = ON;//押されていて，かつリミットスイッチが押されていなかったら左回転
             }
+            
             if(GetBit(INdata[3], 2)){
                 Turn[CW] = ON;//押されたら右回転
             }
         }
     }
 }
+
+void SetUp(void){
+    
+    uint8_t i=1;
+    
+    mt.mode(PullUp);
+    
+    TurnSpeed = TURN_REV;//上体回転のPWM
+    TurnSW.mode(PullUp);
+    
+    HandSW[R].mode(PullUp);
+    HandSW[L].mode(PullUp);
+    HandSW[C].mode(PullUp);
+    HandSW[R].rise(RightCatch);//立ち上がり
+    HandSW[L].rise(LeftCatch);//立ち上がり
+    HandSW[C].rise(CenterCatch);//立ち上がり
+    
+    Xbee.attach(&RX, Serial::RxIrq);
+    Xbee.attach(&TX, Serial::TxIrq);
+    
+    Xbee.putc(i);//送信開始
+    i = Xbee.getc();//受信開始
+}
\ No newline at end of file