Diff: main.cpp

Revision:

3:2184f6f5c8e3

Parent:

2:213c12298d45

Child:

4:29bf39bd97a7

--- a/main.cpp	Thu Sep 21 14:18:33 2017 +0000
+++ b/main.cpp	Thu Sep 21 15:07:25 2017 +0000
@@ -5,7 +5,9 @@
 DigitalOut grab(p11); //0:閉じる 1:開く
 DigitalOut snap(p12); //0:縮む 1:伸びる
 std_msgs::String action;
-
+int k_action=0;
+int o_action=0;
+int m_action=0;
 void messageCallback(const std_msgs::String &msg)
 {
     action.data=msg.data;
@@ -18,7 +20,8 @@
 DigitalOut led3(LED3);
 DigitalOut led4(LED4);
 
-void led_on(int l1,int l2,int l3,int l4){
+void led_on(int l1,int l2,int l3,int l4)
+{
     led1=l1;
     led2=l2;
     led3=l3;
@@ -29,38 +32,36 @@
 {
     nh.initNode();
     nh.subscribe(sub);
-        while(1) {
-            if(*(action.data)== 'k') {
-                snap = 1;
-                grab = 0;
-                led_on(1,0,0,0);
-            }
+    while(1) {
+        if((*(action.data)== 'k')&&(k_action==0)) {
+            snap = 1;
+            grab = 0;
+            k_action++;
+            led_on(1,0,0,0);
+        }
 
-            else  if(*(action.data)== 'o') {
-                grab = 1;
-                led_on(0,1,0,0);
-            }
+        else  if((*(action.data)== 'o')&&(o_action==0)) {
+            grab = 1;
+            o_action++;
+            led_on(0,1,0,0);
+        }
 
-            else if(*(action.data)== 'j') {
-                led_on(0,0,1,0);
-                snap = 1;
-                wait(0.15);
-                grab = 1;
-            }
+        else if((*(action.data)=='m')&&(m_action==0)) {
+            led_on(1,1,0,0);
+            snap = 0;
+            m_action++;
+            //pc.printf("a\n");
+            wait(0.195);             //TZ1:0.18 TZ2:0.18 TZ3:0.167,0.168 椅子(高度45[cm]):0.195
+            grab = 1;
 
-            else if(*(action.data)=='l') {
-                snap = 0;
-                grab = 0;
-                led_on(0,0,0,1);
-            } else if(*(action.data)=='m') {
-                led_on(1,1,0,0);
-                snap = 0;
-                //pc.printf("a\n");
-                wait(0.195);             //TZ1:0.18 TZ2:0.18 TZ3:0.167,0.168 椅子(高度45[cm]):0.195
-                grab = 1;
-                
-            }
-            nh.spinOnce();
         }
-    
+        //一連の動作が終了したかを判断する。
+        if((k_action==1)&&(o_action==1)&&(m_action==1)) {
+            k_action=0;
+            o_action=0;
+            m_action=0;
+        }
+        nh.spinOnce();
+    }
+
 }
\ No newline at end of file