Diff: main.cpp

Revision:

5:4fba3e91741f

Parent:

4:3abefa13c2e3

Child:

6:e6b303ea6439

--- a/main.cpp	Sat Jul 10 06:19:53 2021 +0000
+++ b/main.cpp	Sat Jul 10 09:19:49 2021 +0000
@@ -18,6 +18,8 @@
 #define OMEGA_MAX 0.6       //速度制御を利用した角度制御での角速度の最大値
 #define NUM_DATA 270
 
+int state = 0;
+
 ////角度制御(アブソリュートエンコーダ)/////
 DigitalOut myled1(LED1);
 Timer timer;
@@ -26,17 +28,20 @@
 CalPID angle_omega_pid(5.5,0,0.0032,DELTA_T,OMEGA_MAX);//角度制御（速度式）のPD計算
 Amt21 amt(p9,p10,p8);    //Amt21 (PinName tx,PinName rx,PinName mode):serial_(tx,rx),rs485_mode(mode)
 MotorController motor(p26,p25,DELTA_T,amt,speed_pid,angle_duty_pid,angle_omega_pid);    //MotorController(PinName motor_p_, PinName motor_n_,double dt, Amt21 &ec, CalPID &sc_, CalPID &ac_duty, CalPID &ac_omega)
+double radToDeg(double x);
+double degToRad(double x);
 double target_speed=0;
 double target_rad=0;
 double target_deg = 0;
-double radToDeg(double x);
-double degToRad(double x);
+double right_deg = -90;
+double left_deg = 90;
 
 ////先端アーム(サーボ)/////
 DigitalIn ts_c1(p6, PullUp);
 DigitalIn ts_c2(p7, PullUp);
 KondoServo servo_c(p13, p14);
 double servoDeg(double servo_value);
+void touchGrab();
 int id_c1 = 0;
 float deg_c_grab = 10000;
 float deg_c_open = 7500;
@@ -63,7 +68,11 @@
 
 int main()
 {
-    myled1 = 0;     //アブソリュートエンコーダ用if文中で点灯
+    myled1 = 0;     //目標角度以上離れている:点灯
+    myled2 = 0;     //通信で欲しい値が送られているとき:点灯
+    myled3 = 0;     //
+    myled4 = 0;     //サーボを掴む→点灯．離す→消灯
+
     /////////       ROS       /////////
     nh.getHardware()->setBaud(921600);
     nh.initNode();
@@ -80,39 +89,68 @@
     while (1) {
         ////    ROS     ////
         nh.spinOnce();
-        if(arm_num==1&&arm_deg==30) {       //動作確認用
-            myled2 = 1;
-        }
-        arm_rad = degToRad(arm_deg);
-        target_rad = -arm_rad;
-        target_deg = -arm_deg;
 
-
-        ////タッチセンサ////
-        if(ts_c1 == 0) {      //タッチセンサ反応したらハンドルを掴む
-            wait_us(50);
-            if(ts_c1 == 0) {
-                servo_c.set_degree(0,servoDeg(deg_c_grab));   //servo_c掴む
-                hand = 1;
+        if(timer.read()>DELTA_T) {
+            if(arm_num==1&&arm_deg==30) {       //動作確認用
+                myled2 = 1;
             }
-        }
+            arm_rad = degToRad(arm_deg);
+//        target_rad = -arm_rad;
+//        target_deg = -arm_deg;
+            arm_deg = -30;
+            arm_rad = degToRad(arm_deg);
 
-        ////アブソリュートエンコーダ////
-        if(timer.read()>DELTA_T) {
-            if(fabs(now_deg-target_deg) > 0.5) {      //現在角度と目標角度が0.5°以上離れている間回す
-                myled1 = 1;   //LED1 点灯
-                if(hand == 1) {     //タッチセンサが反応していたとき(ハンドルを掴んでいたとき)
+            if(state == 0) {
+                target_deg = -60;
+                target_rad = degToRad(target_deg);
+                state++;
+            } else if (state == 1) {
+                myled3 = 1;
+                if(fabs(now_deg-target_deg) > 0.5) {      //現在角度と目標角度が0.5°以上離れている間回す
+                    myled1 = 1;   //LED1 点灯
+                    motor.AcOmega(target_rad);   //速度式角度制御
+                } else { //目標角度に達したら停止
+                    myled1=0;   //LED1 消灯
+                    motor.stop();
+                }
+                touchGrab();                //タッチセンサ
+            } else if (state == 2) {
+                myled3 = 0;
+                target_rad = arm_rad;
+                target_deg = arm_deg;
+                state++;
+            } else if (state == 3) {
+                if(fabs(now_deg-target_deg) > 0.5) {      //現在角度と目標角度が0.5°以上離れている間回す
+                    myled1 = 1;   //LED1 点灯
                     motor.AcOmega(target_rad);   //速度式角度制御
-                } else if(hand == 0) {
-                    myled4=1;   //LED4 点灯
+                } else { //目標角度に達したら停止
+                    myled1=0;   //LED1 消灯
+                    motor.stop();
+                    //wait(100);    //table停止用時間
+                    servo_c.set_degree(0,servoDeg(deg_c_open));   //servo_c離す
+                    myled4 = 0;
                 }
-            } else { //目標角度に達したら停止
-                myled1=0;   //LED1 消灯
-                motor.stop();
-                hand = 0;
-                //wait(100);    //table停止用時間
-                servo_c.set_degree(0,servoDeg(deg_c_open));   //servo_c離す
             }
+
+            ////タッチセンサ////
+//        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();
@@ -138,4 +176,16 @@
 double degToRad(double x)
 {
     return x*M_PI/180;
+}
+void touchGrab()
+{
+    if(ts_c1 == 0) {      //タッチセンサ反応したらハンドルを掴む
+        wait_us(50);
+        if(ts_c1 == 0) {
+            servo_c.set_degree(0,servoDeg(deg_c_grab));   //servo_c掴む
+            state++;
+//            hand = 1;
+            myled4=1;   //LED4 点灯
+        }
+    }
 }
\ No newline at end of file