Diff: main.cpp

Revision:

7:2827264ddf53

Parent:

6:ca4fab2957e4

Child:

8:5d8b23cec6d1

--- a/main.cpp	Sat Nov 25 06:04:55 2017 +0000
+++ b/main.cpp	Fri Dec 01 07:48:37 2017 +0000
@@ -2,7 +2,7 @@
 #include <ros.h>
 #include <std_msgs/Int32.h>
 #include <std_msgs/String.h>
-#define RED_MIN 3000
+#define RED_MIN 4000
  
 //s_grab: 1:閉じる,0:開く
 //grab: 1close,2:open
@@ -18,7 +18,9 @@
 DigitalOut led2(LED2);
 DigitalOut led3(LED3);
 DigitalOut led4(LED4);
-PwmOut servo(p21);
+DigitalOut led5(p20);
+DigitalOut led6(p21);
+DigitalOut led7(p22);
 std_msgs::String action;
 std_msgs::String fin_or_not;
 std_msgs::String get_or_not;
@@ -69,7 +71,7 @@
     cmd[0] = 0x0;
     cmd[1] = 0x09;
     val = i2c.write(84, cmd, 2);
-    servo.period_ms(20);
+    //servo.period_ms(20);
     //int jk=0;
     while(1) {
         //wait_ms(50);//最小値ではない
@@ -80,7 +82,13 @@
         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;
             grab=1;
             s_grab=0;
             ready_action=1;
@@ -116,7 +124,7 @@
         }
         //get ready
         //shashutu
-            if((act== 1)&&(count==0)) { 
+            if((act== 1)&&(count==0)) {
                 led1=1;
                 s_grab=1;
                 wait(3);