Diff: main.cpp

Revision:

9:1dcd40da31ec

Parent:

8:5d8b23cec6d1

Child:

10:a05e9a8980db

--- a/main.cpp	Sat Dec 02 02:35:26 2017 +0000
+++ b/main.cpp	Sat Dec 02 04:14:02 2017 +0000
@@ -2,8 +2,8 @@
 #include <ros.h>
 #include <std_msgs/Int32.h>
 #include <std_msgs/String.h>
-#define RED_MIN 2500
-#define R_C_MAX 10
+int RED_MIN=0;
+#define R_C_MAX 50
 //s_grab: 1:閉じる,0:開く
 //grab: 1close,2:open
 //snap:1 slow 0:catch
@@ -62,6 +62,7 @@
     MC.attach(&MCL,0.01);
     pub_act.data=0;
     int r_c=0;
+    int first_up=0;
     char cmd[2];
     char cell[1]= {0x03};
     char data[8]= {0,0,0,0,0,0,0,0};
@@ -79,17 +80,29 @@
         val = i2c.write(84, cell, 1);
         val = i2c.read(84, data, 8);
         red = data[0]<<8 | data[1];
+        if(first_up==0&&red!=0){
+            RED_MIN=red-500;
+            first_up++;}
+        if(red<RED_MIN){
+            led1=1;
+            led2=0;
+            //led5=1;
+            //led6=1;
+            //led7=1;
+            }
+        else{
+            led1=0;
+            led2=1;
+            //led5=0;
+            //led6=0;
+            //led7=0;
+            }
         //pc.printf("%d",red);
+        if(first_up>0){
         if((act==100)&&(ready_action==0)) {
             snap=1;
             grab=0;
-            led5=1;
-            led6=0;
-            led7=0;
             if(red<RED_MIN){
-            led5=0;
-            led6=1;
-            led7=0;
             r_c++;
             }
             else r_c=0;
@@ -104,13 +117,10 @@
             }
         }
         if((act==2)&&(ready_action==0)) {
-            led2=1;
+            //led2=1;
             snap=1;
             grab=0;
             if(red<RED_MIN){
-            led5=0;
-            led6=1;
-            led7=0;
             r_c++;
             }
             else r_c=0;
@@ -128,9 +138,6 @@
             snap=1;
             grab=0;
             if(red<RED_MIN){
-            led5=0;
-            led6=1;
-            led7=0;
             r_c++;
             }
             else r_c=0;
@@ -147,9 +154,13 @@
         //get ready
         //shashutu
             if((act== 1)&&(count==0)) {
-                led1=1;
+                //led1=1;
+                //led6=1;
+                //led7=1;
                 s_grab=1;
-                wait(3);
+                wait(1);
+                led5=1;
+                wait(9);
                 snap=0;
                 wait(0.183); //TZ1
                 grab=0;
@@ -157,10 +168,17 @@
                 count=1;
                 pub_act.data=act;
                 pub_action.publish(&pub_act);
+                led5=0;
+                //led6=0;
+                //led7=0;
             }
             if((act== 3)&&(count==0)) {
+                //led6=1;
+                //led7=1;
                 s_grab=1;
-                wait(3);
+                wait(1);
+                led5=1;
+                wait(9);
                 snap=0;
                 wait(0.183); //TZ1
                 grab=0;
@@ -168,12 +186,19 @@
                 count=1;
                 pub_act.data=act;
                 pub_action.publish(&pub_act);
+                led5=0;
+                //led6=0;
+                //led7=0;
             }
  
             if((act==4)&&(count==0)) {
-                led4=1;
+                //led4=1;
+                //led6=1;
+                //led7=1;
                 s_grab=1;
-                wait(3);
+                wait(1);
+                led5=1;
+                wait(9);
                 snap=0;
                 wait(0.182); //TZ2
                 grab=0;
@@ -181,10 +206,15 @@
                 count=1;
                 pub_act.data=act;
                 pub_action.publish(&pub_act);
+                led5=0;
+                //led6=0;
+                //led7=0;
             }
             if((act==6)&&(count==0)) {
                 s_grab=1;
-                wait(3);
+                wait(1);
+                led5=1;
+                wait(9);
                 snap=0;
                 wait(0.182); //TZ2
                 grab=0;
@@ -192,10 +222,13 @@
                 count=1;
                 pub_act.data=act;
                 pub_action.publish(&pub_act);
+                led5=0;
             }
             if((act==7)&&(count==0)) {
                 s_grab=1;
-                wait(3);
+                wait(1);
+                led5=1;
+                wait(9);
                 snap=0;
                 wait(0.152); //TZ3
                 grab=0;
@@ -203,6 +236,7 @@
                 count=1;
                 pub_act.data=act;
                 pub_action.publish(&pub_act);
+                led5=0;
             }
             //一連の動作が終了したかを判断する。
             if(count>0) {
@@ -218,5 +252,5 @@
         
         //if(pub_act.data!=act)pub_act.data=0;
     }
- 
+ }
 }
\ No newline at end of file