Diff: main.cpp

Revision:

7:038ea29c5a97

Parent:

6:e6b303ea6439

Child:

8:9656790eea17

--- a/main.cpp	Mon Jul 19 13:20:31 2021 +0000
+++ b/main.cpp	Tue Jul 20 05:02:52 2021 +0000
@@ -35,6 +35,8 @@
 double target_deg = 0;
 double right_deg = -90;
 double left_deg = 90;
+double turn_deg = 0;
+double turn_rad = 0;
 
 ////先端アーム(サーボ)/////
 DigitalIn ts_c1(p6, PullUp);
@@ -57,10 +59,10 @@
 DigitalOut myled2(LED2);
 DigitalOut myled3(LED3);
 DigitalOut myled4(LED4);
-float arm_num=0,arm_deg=0,arm_rad=0;
+float arm_turn=0,arm_deg=0,arm_rad=0;
 void cArmCallback(const geometry_msgs::Point &arm_state)
 {
-    arm_num = arm_state.x;        //右回転＝1、左回転＝2
+    arm_turn = arm_state.x;        //右回転＝1、左回転＝2
     arm_deg = arm_state.y;        //目標角度
 }
 ros::NodeHandle  nh;
@@ -94,14 +96,12 @@
         nh.spinOnce();
 
         if(timer.read()>DELTA_T) {
-            if(arm_num==1&&arm_deg==30) {       //動作確認用
+            if(arm_turn==2&&arm_deg==-10) {       //動作確認用
                 myled2 = 1;
             }
             arm_rad = degToRad(arm_deg);
-//            target_rad = -arm_rad;
-//            target_deg = -arm_deg;
-            arm_deg = -15;
-            arm_rad = degToRad(arm_deg);
+            target_deg = arm_deg;
+            target_rad = arm_rad;
 
             if(state == 0) {
                 ///トグルスイッチ1////1
@@ -113,15 +113,19 @@
                 }
             }
             if(state == 1) {
-                target_deg = -90;
-                target_rad = degToRad(target_deg);
+                if(arm_turn == 1) {
+                    turn_deg = 90;
+                } else if(arm_turn == 2) {
+                    turn_deg = -90;
+                }
+                turn_rad = degToRad(turn_deg);
                 servo_c.set_degree(0,servoDeg(deg_c_open));   //servo_c離す
                 state++;
             } else if (state == 2) {
                 myled3 = 1;
-                if(fabs(now_deg-target_deg) > 0.5) {      //現在角度と目標角度が0.5°以上離れている間回す
+                if(fabs(now_deg-turn_deg) > 0.5) {      //現在角度と目標角度が0.5°以上離れている間回す
                     myled1 = 1;   //LED1 点灯
-                    motor.AcOmega(target_rad);   //速度式角度制御
+                    motor.AcOmega(turn_rad);   //速度式角度制御
                 } else { //目標角度に達したら停止
                     myled1=0;   //LED1 消灯
                     motor.stop();
@@ -198,7 +202,6 @@
         if(ts_c1 == 0) {
             servo_c.set_degree(0,servoDeg(deg_c_grab));   //servo_c掴む
             state++;
-//            hand = 1;
             myled4=1;   //LED4 点灯
         }
     }