Diff: main.cpp

Revision:

2:eb1c123c9140

Parent:

1:006debaed874

Child:

3:a1489a94c436

--- a/main.cpp	Mon Jul 14 09:18:29 2014 +0000
+++ b/main.cpp	Wed Sep 03 05:22:12 2014 +0000
@@ -6,7 +6,8 @@
 #define KEY 0//number of first data
 #define CHECK DATA_NUM-1//number of last data
 #define TIME_LIMIT 1.0//腕が閉まっても動けるのはTIME_LIMIT[s]まで
-#define ARM_REV .25
+#define ARM_REV 1.0
+#define TURN_REV 0.5
 
 volatile uint8_t INdata[DATA_NUM]={0}, EXdata[DATA_NUM]={0};
 volatile uint8_t count=0;
@@ -14,17 +15,25 @@
 enum Logic{OFF=0, ON};//殆どはモーターのON，OFFに使う
 enum Arm{R=0, L, C};//右腕と左腕
 enum Clip{Open=0, Close};//開ける動作と閉める動作
+enum Revolution{CW=0, CCW};//正回転，逆回転(上体回転)
+enum Compass{N=0, E, W, S};//マシンの回転
 
-bool GetBit(uint8_t n, uint8_t bit){//n=8[bit]data, bit = 1-8(8bitのデータから指定bitの数値を読み取る)
+bool GetBit(uint8_t n, uint8_t bit){//n=8[bit]data, bit = 1-8(8bitのデータから指定bitの数値を読み取る)-
     return (n>>(bit-1))%2;
 }
 
 BusOut LED(LED1, LED2, LED3, LED4);//確認用
-BusIn mt(p5, p6, p7, p8, p9, p10);//Switch
-InterruptIn HandSW[3]={p15, p16, p17};//p14, p15, p16//R, L, C
-PwmOut HandSpeed[2]={p25, p26};
-DigitalOut Hand[2][2]={{p30, p29}, {p28, p27}};//{p17, p18}, {p19, p20}
+BusIn mt(p19, p20, p17, p18);//Switch
+//DigitalIn Ninety[2]={p5, p6};//R, L
+DigitalIn OneEighty[2]={p5, p6};//R, L
+InterruptIn HandSW[3]={p7, p8, p9};//p14, p15, p16//R, L, C
+DigitalIn TurnSW[2]={p11, p12};//
+PwmOut HandSpeed[2]={p23, p24};
+PwmOut TurnSpeed(p22);//p22
+DigitalOut Hand[2][2]={{p28, p27}, {p26, p25}};//{p17, p18}, {p19, p20}
 //ex)Hand[R][Close]=ON;
+DigitalOut Turn[2]={p29, p30};//p29, p30
+//ex)Turn[CW]=ON;
 Serial pc(USBTX, USBRX);
 Ticker flick;
 Timeout Touch;//腕で掴むときの安全対策
@@ -54,7 +63,8 @@
             if(GetBit(INdata[1], 2)==true){//腕を広げる動作はマニュアル操作
                 StopCatching();//停止
                 Touch.detach();//ワンショットタイマー停止
-                Hand[R][Open] = Hand[L][Open] = GetBit(INdata[1], 2);
+                Hand[R][Open] = GetBit(INdata[1], 2)&&OneEighty[R];
+                Hand[L][Open] = GetBit(INdata[1], 2)&&OneEighty[L];
             }
         }
         
@@ -63,6 +73,29 @@
             Touch.detach();//ワンショットタイマー停止
         }
         
+        if(!OneEighty[R]) Hand[R][Open]=OFF;
+        if(!OneEighty[L]) Hand[L][Open]=OFF;
+        //if(!Ninety[R]) Hand[R][Close]=OFF;
+        //if(!Ninety[L]) Hand[L][Close]=OFF;
+        
+        //if((!Ninety[R])&&(!Ninety[L])){//どちらも押されたら停止
+        //    StopCatching();//停止
+        //    Touch.detach();//ワンショットタイマー停止
+        //}
+        
+        LED = ((0x01 * !HandSW[R]) | (0x02 * !HandSW[L]) | (0x04 * !HandSW[C]) | (0x08 * ((!OneEighty[R])&&(!OneEighty[L])) ));
+        
+        //上体回転
+        
+        if(GetBit(INdata[1], 3)==OFF) Turn[CW] = OFF;//停止
+        if(GetBit(INdata[1], 4)==OFF) Turn[CCW] = OFF;//停止
+        
+        if((GetBit(INdata[1], 3)+GetBit(INdata[1], 4))==1){//同時押し防止
+            
+            Turn[CW] = GetBit(INdata[1], 3)&&TurnSW[N];//押されていて，かつリミットスイッチが押されていなかったら右回転
+            Turn[CCW] = GetBit(INdata[1], 4)&&TurnSW[E];//押されていて，かつリミットスイッチが押されていなかったら左回転
+            
+        }
     }
 }
 
@@ -72,11 +105,25 @@
 void StopLeaving(void){//停止
     Hand[R][Open]=Hand[L][Open]=OFF;
 }
-void StartCatching(void){//掴む
-    Hand[R][Close]=Hand[L][Close]=ON;
+void StartCatching(void){//掴む（リミットスイッチが押されてなければ）
+
+    if((!HandSW[R])&&(!HandSW[L])){
+        Hand[R][Close] = Hand[L][Close] = OFF;
+    }
+    else{
+        
+        //Hand[R][Close]=Ninety[R];
+        //Hand[L][Close]=Ninety[L];
+        
+        Hand[R][Close]=ON;
+        Hand[L][Close]=ON;
+            
+    }
+    
 }
-void StartLeaving(void){//離す
-    Hand[R][Open]=Hand[L][Open]=ON;
+void StartLeaving(void){//離す（リミットスイッチが押されてなければ）
+    Hand[R][Open]=OneEighty[R];
+    Hand[L][Open]=OneEighty[L];
 }
 void RightCatch(void){//右腕のリミットスイッチ
     if((!HandSW[R])&&(!HandSW[L])){//どちらも押されたら停止
@@ -91,8 +138,12 @@
     }
 }
 void CenterCatch(void){//中央のリミットスイッチ
-    Touch.attach(StopCatching, TIME_LIMIT);//TIME_LIMIT[s]で停止
-    StartCatching();
+
+    if((!OneEighty[R])&&(!OneEighty[L])){
+        Touch.attach(StopCatching, TIME_LIMIT);//TIME_LIMIT[s]で停止
+        StartCatching();
+    }
+    
 }
 void illumination(void){//literally
     count++;
@@ -103,8 +154,9 @@
     mt.mode(PullUp);
     
     HandSpeed[R] = HandSpeed[L] = ARM_REV;
+    TurnSpeed = TURN_REV;
     
-    flick.attach(illumination, .5);
+    //flick.attach(illumination, .5);
     
     HandSW[R].mode(PullUp);
     HandSW[L].mode(PullUp);
@@ -112,4 +164,13 @@
     HandSW[R].fall(RightCatch);
     HandSW[L].fall(LeftCatch);
     HandSW[C].fall(CenterCatch);
+    
+    OneEighty[R].mode(PullUp);
+    OneEighty[L].mode(PullUp);
+    //Ninety[R].mode(PullUp);
+    //Ninety[L].mode(PullUp);
+    
+    TurnSW[N].mode(PullUp);
+    TurnSW[E].mode(PullUp);
+    
 }
\ No newline at end of file