Diff: main.cpp

Revision:

8:5d8b23cec6d1

Parent:

7:2827264ddf53

Child:

9:1dcd40da31ec

--- a/main.cpp	Fri Dec 01 07:48:37 2017 +0000
+++ b/main.cpp	Sat Dec 02 02:35:26 2017 +0000
@@ -2,8 +2,8 @@
 #include <ros.h>
 #include <std_msgs/Int32.h>
 #include <std_msgs/String.h>
-#define RED_MIN 4000
- 
+#define RED_MIN 2500
+#define R_C_MAX 10
 //s_grab: 1:閉じる,0:開く
 //grab: 1close,2:open
 //snap:1 slow 0:catch
@@ -61,6 +61,7 @@
     nh.advertise(pub_action);
     MC.attach(&MCL,0.01);
     pub_act.data=0;
+    int r_c=0;
     char cmd[2];
     char cell[1]= {0x03};
     char data[8]= {0,0,0,0,0,0,0,0};
@@ -89,12 +90,17 @@
             led5=0;
             led6=1;
             led7=0;
+            r_c++;
+            }
+            else r_c=0;
+            if(r_c>R_C_MAX){
             grab=1;
             s_grab=0;
             ready_action=1;
             count=0;
             pub_act.data=act;
             pub_action.publish(&pub_act);
+            r_c=0;
             }
         }
         if((act==2)&&(ready_action==0)) {
@@ -102,24 +108,40 @@
             snap=1;
             grab=0;
             if(red<RED_MIN){
+            led5=0;
+            led6=1;
+            led7=0;
+            r_c++;
+            }
+            else r_c=0;
+            if(r_c>R_C_MAX){
             grab=1;
             s_grab=0;
             ready_action=1;
             count=0;
             pub_act.data=act;
             pub_action.publish(&pub_act);
+            r_c=0;
             }
         }
         if((act==5)&&(ready_action==0)) {  
             snap=1;
             grab=0;
             if(red<RED_MIN){
+            led5=0;
+            led6=1;
+            led7=0;
+            r_c++;
+            }
+            else r_c=0;
+            if(r_c>R_C_MAX){
             grab=1;
             s_grab=0;
             ready_action=1;
             count=0;
             pub_act.data=act;
             pub_action.publish(&pub_act);
+            r_c=0;
             }
         }
         //get ready
@@ -129,7 +151,7 @@
                 s_grab=1;
                 wait(3);
                 snap=0;
-                wait(0.17); //TZ1
+                wait(0.183); //TZ1
                 grab=0;
                 ready_action=0;
                 count=1;
@@ -140,7 +162,7 @@
                 s_grab=1;
                 wait(3);
                 snap=0;
-                wait(0.17); //TZ1
+                wait(0.183); //TZ1
                 grab=0;
                 ready_action=0;
                 count=1;
@@ -153,7 +175,7 @@
                 s_grab=1;
                 wait(3);
                 snap=0;
-                wait(0.17); //TZ2
+                wait(0.182); //TZ2
                 grab=0;
                 ready_action=0;
                 count=1;
@@ -164,7 +186,7 @@
                 s_grab=1;
                 wait(3);
                 snap=0;
-                wait(0.17); //TZ2
+                wait(0.182); //TZ2
                 grab=0;
                 ready_action=0;
                 count=1;
@@ -175,7 +197,7 @@
                 s_grab=1;
                 wait(3);
                 snap=0;
-                wait(0.17); //TZ3
+                wait(0.152); //TZ3
                 grab=0;
                 ready_action=0;
                 count=1;