Diff: main.cpp

Revision:

12:c2bb0a9587b5

Parent:

11:df2acf22123e

--- a/main.cpp	Sun Jul 25 13:57:43 2021 +0000
+++ b/main.cpp	Fri Jul 30 11:36:46 2021 +0000
@@ -48,6 +48,8 @@
 double now_deg=0;
 double now_count=0;
 
+int catch_mode = 0;
+
 /////////////////               ROS               /////////////////
 DigitalOut myled1(LED1);
 DigitalOut myled2(LED2);
@@ -58,7 +60,9 @@
 {
     arm_turn = arm_state.x;     //左回転＝1、右回転＝2
     arm_deg = arm_state.y;      //目標角度
+    catch_mode = arm_state.z;
 }
+
 ros::NodeHandle  nh;
 ros::Subscriber<geometry_msgs::Point> sub("/arm",&cArmCallback);
 
@@ -86,6 +90,15 @@
 
     while (1) {
         nh.spinOnce();      //ROS
+        
+        if(catch_mode == 1){
+            servo_c.set_degree(0,servoDeg(deg_c_open)); //servo_c離す
+            myled1 = 1;
+        }
+        else if(catch_mode == 2){
+            servo_c.set_degree(0,servoDeg(deg_c_grab));   //servo_c掴む
+            myled2 = 1;
+        }
 
         if(timer.read()>DELTA_T) {
             if(arm_turn==2&&arm_deg==-10) {       //動作確認(通信)用
@@ -125,7 +138,8 @@
                     myled1=0;   //LED1 消灯
                     motor.stop();
                 }
-                touchGrab();
+                state++;
+                //touchGrab();
             } else if (state == 4) {//目標角度を設定してそのまますぐstate=5へ．
                 myled3 = 0;
                 if(mode == 0){
@@ -141,7 +155,7 @@
                     target_rad = degToRad(target_deg);   
                 }
                 state++;
-            } else if (state == 5) {//目標角度に近づいたら停止してサーボを離す．
+            } else if (state == 5) {//目標角度に近づいたら停止する
                 if(fabs(now_deg-target_deg) > 0.5) {        //現在角度と目標角度が0.5°以上離れている間回す
                     myled1 = 1; //LED1 点灯
                     motor.AcOmega(target_rad);              //速度式角度制御