Diff: main.cpp

Revision:

8:9656790eea17

Parent:

7:038ea29c5a97

Child:

9:95f6b9f54395

--- a/main.cpp	Tue Jul 20 05:02:52 2021 +0000
+++ b/main.cpp	Tue Jul 20 06:41:29 2021 +0000
@@ -33,8 +33,6 @@
 double target_speed=0;
 double target_rad=0;
 double target_deg = 0;
-double right_deg = -90;
-double left_deg = 90;
 double turn_deg = 0;
 double turn_rad = 0;
 
@@ -62,7 +60,7 @@
 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_turn = arm_state.x;        //左回転＝1、右回転＝2
     arm_deg = arm_state.y;        //目標角度
 }
 ros::NodeHandle  nh;
@@ -75,7 +73,7 @@
 {
     myled1 = 0;     //目標角度以上離れている:点灯
     myled2 = 0;     //通信で欲しい値が送られているとき:点灯
-    myled3 = 0;     //
+    myled3 = 0;     //state2に入ったとき←基本不要
     myled4 = 0;     //サーボを掴む→点灯．離す→消灯
 
     /////////       ROS       /////////
@@ -88,15 +86,11 @@
 
     motor.setEquation(0.0523,0.0148,0.0459,-0.0341);    //Excel"omega_controll"の図より
 
-//    target_rad = -(M_PI/6); //目標角度をπ/6（rad）に（単位に注意）           //////////////////////////
-//    target_deg = radToDeg(target_rad);
-
     while (1) {
-        ////    ROS     ////
-        nh.spinOnce();
+        nh.spinOnce();      //ROS
 
         if(timer.read()>DELTA_T) {
-            if(arm_turn==2&&arm_deg==-10) {       //動作確認用
+            if(arm_turn==2&&arm_deg==-10) {       //動作確認(通信)用
                 myled2 = 1;
             }
             arm_rad = degToRad(arm_deg);
@@ -104,7 +98,7 @@
             target_rad = arm_rad;
 
             if(state == 0) {
-                ///トグルスイッチ1////1
+                ///トグルスイッチ1////マイコン方向
                 if(toggle1 == 0) {                  //トグルスイッチ1(mbed側)ONで初start
                     wait_us(50);
                     if(toggle1 == 0) {
@@ -143,38 +137,16 @@
                 } else { //目標角度に達したら停止
                     myled1=0;   //LED1 消灯
                     motor.stop();
-                    //wait(100);    //table停止用時間
                     servo_c.set_degree(0,servoDeg(deg_c_open));   //servo_c離す
                     myled4 = 0;
 //                    state = 0;
                 }
             }
 
-            ////タッチセンサ////
-//        touchGrab();
-
-            ////アブソリュートエンコーダ////
-
-//            if(fabs(now_deg-target_deg) > 0.5) {      //現在角度と目標角度が0.5°以上離れている間回す
-//                myled1 = 1;   //LED1 点灯
-////                if(hand == 1) {     //タッチセンサが反応していたとき(ハンドルを掴んでいたとき)
-//                motor.AcOmega(target_rad);   //速度式角度制御
-////                } else if(hand == 0) {
-////                    myled4=0;   //LED4 消灯
-////                }
-//            } else if(fabs(now_deg-target_deg) < 0.5 && state == 3) { //目標角度に達したら停止
-//                myled1=0;   //LED1 消灯
-//                motor.stop();
-//                hand = 0;
-//                //wait(100);    //table停止用時間
-//                servo_c.set_degree(0,servoDeg(deg_c_open));   //servo_c離す
-//            }
             amt.rewriteCount();
             amt.calOmega();
-            amt.getOmega();
             now_deg = amt.getDeg();
             now_count=amt.getCount();
-//            motor.AcOmega(target_rad);
 
             timer.reset();
         }