Diff: main.cpp

Revision:

10:c1ca3db7177c

Parent:

9:95f6b9f54395

Child:

11:df2acf22123e

--- a/main.cpp	Tue Jul 20 08:57:36 2021 +0000
+++ b/main.cpp	Tue Jul 20 12:02:23 2021 +0000
@@ -40,7 +40,7 @@
 DigitalIn ts_c2(p7, PullUp);
 KondoServo servo_c(p13, p14);
 double servoDeg(double servo_value);
-void touchGrab();   //タッチセンサ
+void touchGrab();   //タッチセンサ反応したらサーボ掴む
 float deg_c_grab = 10600;
 float deg_c_open = 8300;
 
@@ -56,8 +56,8 @@
 float arm_turn=0,arm_deg=0,arm_rad=0;
 void cArmCallback(const geometry_msgs::Point &arm_state)
 {
-    arm_turn = arm_state.x;        //左回転＝1、右回転＝2
-    arm_deg = arm_state.y;        //目標角度
+    arm_turn = arm_state.x;     //左回転＝1、右回転＝2
+    arm_deg = arm_state.y;      //目標角度
 }
 ros::NodeHandle  nh;
 ros::Subscriber<geometry_msgs::Point> sub("/arm",&cArmCallback);
@@ -90,49 +90,52 @@
             target_deg = arm_deg;
             target_rad = arm_rad;
 
-            if(state == 0) {
+            if(state == 0) {        //トグルスイッチONでstate=1へ．
                 ///トグルスイッチ1////マイコン方向
-                if(toggle1 == 0) {                  //トグルスイッチ1(mbed側)ONで初start
+                if(toggle1 == 0) {  //トグルスイッチ1(mbed側)ONで初start
                     wait_us(50);
                     if(toggle1 == 0) {
-                        state ++;
+                        state++;
                     }
                 }
             }
-            if(state == 1) {
+            if(state == 1) {        //回転方向の変数"arm_turn"に1か2が代入されたらstate=2へ．
                 if(arm_turn == 1) {
                     turn_deg = 90;
+                    state++;
                 } else if(arm_turn == 2) {
                     turn_deg = -90;
+                    state++;
                 }
                 turn_rad = degToRad(turn_deg);
-                servo_c.set_degree(0,servoDeg(deg_c_open));   //servo_c離す
+            } else if(state == 2) { //サーボを離してそのまますぐstate=3へ．
+                servo_c.set_degree(0,servoDeg(deg_c_open)); //servo_c離す
                 state++;
-            } else if (state == 2) {
+            } else if (state == 3) {//タッチセンサがハンドルに反応したらサーボで掴んでstate=4へ．
                 myled3 = 1;
-                if(fabs(now_deg-turn_deg) > 0.5) {      //現在角度と目標角度が0.5°以上離れている間回す
-                    myled1 = 1;   //LED1 点灯
-                    motor.AcOmega(turn_rad);   //速度式角度制御
+                if(fabs(now_deg-turn_deg) > 0.5) {          //現在角度と目標角度が0.5°以上離れている間回す
+                    myled1 = 1; //LED1 点灯
+                    motor.AcOmega(turn_rad);                //速度式角度制御
                 } else { //目標角度に達したら停止
                     myled1=0;   //LED1 消灯
                     motor.stop();
                 }
-                touchGrab();                //タッチセンサ
-            } else if (state == 3) {
+                touchGrab();
+            } else if (state == 4) {//目標角度を設定してそのまますぐstate=5へ．
                 myled3 = 0;
                 target_rad = arm_rad;
                 target_deg = arm_deg;
                 state++;
-            } else if (state == 4) {
-                if(fabs(now_deg-target_deg) > 0.5) {      //現在角度と目標角度が0.5°以上離れている間回す
-                    myled1 = 1;   //LED1 点灯
-                    motor.AcOmega(target_rad);   //速度式角度制御
+            } else if (state == 5) {//目標角度に近づいたら停止してサーボを離す．
+                if(fabs(now_deg-target_deg) > 0.5) {        //現在角度と目標角度が0.5°以上離れている間回す
+                    myled1 = 1; //LED1 点灯
+                    motor.AcOmega(target_rad);              //速度式角度制御
                 } else { //目標角度に達したら停止
-                    myled1=0;   //LED1 消灯
+                    myled1 = 0; //LED1 消灯
                     motor.stop();
                     servo_c.set_degree(0,servoDeg(deg_c_open));   //servo_c離す
                     myled4 = 0;
-//                    state = 0;
+//                    state = 0;    /////////////////////連続して動かす時はここのコメントアウト外す．
                 }
             }