Diff: main.cpp

Revision:

1:5dd2f53a286f

Parent:

0:f6fa58c56955

Child:

2:213c12298d45

--- a/main.cpp	Fri Jun 02 14:34:33 2017 +0000
+++ b/main.cpp	Wed Sep 13 17:11:59 2017 +0000
@@ -1,22 +1,52 @@
 #include <mbed.h>
 #include <ros.h>
 #include <std_msgs/String.h>
-#include <geometry_msgs/PoseStamped.h>
 
 ros::NodeHandle nh;
-geometry_msgs::Pose pose;
-void messageCallback(const geometry_msgs::PoseStamped & msg){
-    pose=msg.pose;
-    printf("I GOT [%f],[%f],[%f]",pose.position.x,pose.position.y,pose.position.z);
-    }
+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;
+}
 
 
-int main(int argc, char **argv){
-    nh.initNode();
+int main(int argc, char **argv)
+{
+   nh.initNode();
     ros::NodeHandle nh;
-    ros::Subscriber<geometry_msgs::PoseStamped> sub("lrf_pose",&messageCallback);
-    while(1){   
-    nh.spinOnce();
-    wait_ms(1000);
+    ros::Subscriber<std_msgs::String> sub("shoot_action",&messageCallback);
+    if(received==true) {
+        while(1) {
+            if(*(action.data)== 'k') {
+                snap = 1;
+                grab = 0;
+            }
+
+            else  if(*(action.data)== 'o') {
+                grab = 1;
+            }
+
+            else if(*(action.data)== 'j') {
+                snap = 1;
+                wait(0.15);
+                grab = 1;
+            }
+
+            else if(*(action.data)=='l') {
+                snap = 0;
+                grab = 0;
+            } else if(*(action.data)=='m') {
+                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