Diff: main.cpp

Revision:

2:213c12298d45

Parent:

1:5dd2f53a286f

Child:

3:2184f6f5c8e3

--- a/main.cpp	Wed Sep 13 17:11:59 2017 +0000
+++ b/main.cpp	Thu Sep 21 14:18:33 2017 +0000
@@ -2,35 +2,47 @@
 #include <ros.h>
 #include <std_msgs/String.h>
 
-ros::NodeHandle nh;
-bool received=false;
 DigitalOut grab(p11); //0:閉じる 1:開く
 DigitalOut snap(p12); //0:縮む 1:伸びる
 std_msgs::String action;
+
 void messageCallback(const std_msgs::String &msg)
 {
     action.data=msg.data;
-    received=true;
 }
 
+ros::NodeHandle nh;
+ros::Subscriber<std_msgs::String> sub("shoot_action",&messageCallback);
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+DigitalOut led3(LED3);
+DigitalOut led4(LED4);
+
+void led_on(int l1,int l2,int l3,int l4){
+    led1=l1;
+    led2=l2;
+    led3=l3;
+    led4=l4;
+}
 
 int main(int argc, char **argv)
 {
-   nh.initNode();
-    ros::NodeHandle nh;
-    ros::Subscriber<std_msgs::String> sub("shoot_action",&messageCallback);
-    if(received==true) {
+    nh.initNode();
+    nh.subscribe(sub);
         while(1) {
             if(*(action.data)== 'k') {
                 snap = 1;
                 grab = 0;
+                led_on(1,0,0,0);
             }
 
             else  if(*(action.data)== 'o') {
                 grab = 1;
+                led_on(0,1,0,0);
             }
 
             else if(*(action.data)== 'j') {
+                led_on(0,0,1,0);
                 snap = 1;
                 wait(0.15);
                 grab = 1;
@@ -39,14 +51,16 @@
             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();
-            received=false;
         }
-    }
+    
 }
\ No newline at end of file