Diff: main.cpp

Revision:

11:df2acf22123e

Parent:

10:c1ca3db7177c

--- a/main.cpp	Tue Jul 20 12:02:23 2021 +0000
+++ b/main.cpp	Sun Jul 25 13:57:43 2021 +0000
@@ -18,7 +18,7 @@
 #define OMEGA_MAX 0.25       //速度制御を利用した角度制御での角速度の最大値
 #define NUM_DATA 270
 
-int state = 0;
+int state = 1;
 
 ////角度制御(アブソリュートエンコーダ)/////
 Timer timer;
@@ -41,7 +41,7 @@
 KondoServo servo_c(p13, p14);
 double servoDeg(double servo_value);
 void touchGrab();   //タッチセンサ反応したらサーボ掴む
-float deg_c_grab = 10600;
+float deg_c_grab = 11000;
 float deg_c_open = 8300;
 
 ////データ記録関連/////
@@ -64,13 +64,14 @@
 
 int main()
 {
+    int mode = 0;
     myled1 = 0;     //目標角度以上離れている:点灯
     myled2 = 0;     //通信で欲しい値が送られているとき:点灯
     myled3 = 0;     //state2に入ったとき←基本不要
     myled4 = 0;     //サーボを掴む→点灯．離す→消灯
 
     /////////       ROS       /////////
-    nh.getHardware()->setBaud(921600);
+    nh.getHardware()->setBaud(115200);
     nh.initNode();
     nh.subscribe(sub);
 
@@ -78,6 +79,10 @@
     timer.start();
 
     motor.setEquation(0.0523,0.0148,0.0459,-0.0341);    //Excel"omega_controll"の図より
+    ////////////////////////////////////////
+    /*arm_turn = 2;     //左回転＝1、右回転＝2
+    arm_deg = -90;      //目標角度*/
+///////////////////////////////////////
 
     while (1) {
         nh.spinOnce();      //ROS
@@ -123,8 +128,18 @@
                 touchGrab();
             } else if (state == 4) {//目標角度を設定してそのまますぐstate=5へ．
                 myled3 = 0;
-                target_rad = arm_rad;
-                target_deg = arm_deg;
+                if(mode == 0){
+                    target_rad = arm_rad;
+                    target_deg = arm_deg;
+                }
+                else if(mode == 1){
+                    target_deg = 45;
+                    target_rad = degToRad(target_deg);   
+                }
+                else if(mode == 2){
+                    target_deg = -45;
+                    target_rad = degToRad(target_deg);   
+                }
                 state++;
             } else if (state == 5) {//目標角度に近づいたら停止してサーボを離す．
                 if(fabs(now_deg-target_deg) > 0.5) {        //現在角度と目標角度が0.5°以上離れている間回す
@@ -133,9 +148,15 @@
                 } else { //目標角度に達したら停止
                     myled1 = 0; //LED1 消灯
                     motor.stop();
-                    servo_c.set_degree(0,servoDeg(deg_c_open));   //servo_c離す
+                    //servo_c.set_degree(0,servoDeg(deg_c_open));   //servo_c離す
                     myled4 = 0;
-//                    state = 0;    /////////////////////連続して動かす時はここのコメントアウト外す．
+                    if(mode == 0){
+                        mode = 1;
+                    }
+                    else{
+                        mode = 3-mode;
+                    }
+                    state = 4;    /////////////////////連続して動かす時はここのコメントアウト外す．
                 }
             }