Diff: main.cpp

Revision:

6:e6b303ea6439

Parent:

5:4fba3e91741f

Child:

7:038ea29c5a97

--- a/main.cpp	Sat Jul 10 09:19:49 2021 +0000
+++ b/main.cpp	Mon Jul 19 13:20:31 2021 +0000
@@ -15,7 +15,7 @@
 #endif
 #define DELTA_T 0.01        //制御周期
 #define DUTY_MAX 0.07       //duty比の最大値
-#define OMEGA_MAX 0.6       //速度制御を利用した角度制御での角速度の最大値
+#define OMEGA_MAX 0.25       //速度制御を利用した角度制御での角速度の最大値
 #define NUM_DATA 270
 
 int state = 0;
@@ -25,7 +25,7 @@
 Timer timer;
 CalPID speed_pid(0.08,0,0.0008,DELTA_T,DUTY_MAX);   //速度制御のPD計算
 CalPID angle_duty_pid(0,0,0,DELTA_T,DUTY_MAX);   //角度制御（duty式）のPD計算
-CalPID angle_omega_pid(5.5,0,0.0032,DELTA_T,OMEGA_MAX);//角度制御（速度式）のPD計算
+CalPID angle_omega_pid(5.0,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);
@@ -43,8 +43,8 @@
 double servoDeg(double servo_value);
 void touchGrab();
 int id_c1 = 0;
-float deg_c_grab = 10000;
-float deg_c_open = 7500;
+float deg_c_grab = 10600;
+float deg_c_open = 8300;
 int hand = 0;   //タッチセンサがONになった状態:1，OFF:0
 
 ////データ記録まわり/////
@@ -66,6 +66,9 @@
 ros::NodeHandle  nh;
 ros::Subscriber<geometry_msgs::Point> sub("/arm",&cArmCallback);
 
+DigitalIn toggle1(p16, PullUp);     //right
+DigitalIn toggle2(p20, PullUp);     //left
+
 int main()
 {
     myled1 = 0;     //目標角度以上離れている:点灯
@@ -95,16 +98,26 @@
                 myled2 = 1;
             }
             arm_rad = degToRad(arm_deg);
-//        target_rad = -arm_rad;
-//        target_deg = -arm_deg;
-            arm_deg = -30;
+//            target_rad = -arm_rad;
+//            target_deg = -arm_deg;
+            arm_deg = -15;
             arm_rad = degToRad(arm_deg);
 
             if(state == 0) {
-                target_deg = -60;
+                ///トグルスイッチ1////1
+                if(toggle1 == 0) {                  //トグルスイッチ1(mbed側)ONで初start
+                    wait_us(50);
+                    if(toggle1 == 0) {
+                        state ++;
+                    }
+                }
+            }
+            if(state == 1) {
+                target_deg = -90;
                 target_rad = degToRad(target_deg);
+                servo_c.set_degree(0,servoDeg(deg_c_open));   //servo_c離す
                 state++;
-            } else if (state == 1) {
+            } else if (state == 2) {
                 myled3 = 1;
                 if(fabs(now_deg-target_deg) > 0.5) {      //現在角度と目標角度が0.5°以上離れている間回す
                     myled1 = 1;   //LED1 点灯
@@ -114,12 +127,12 @@
                     motor.stop();
                 }
                 touchGrab();                //タッチセンサ
-            } else if (state == 2) {
+            } else if (state == 3) {
                 myled3 = 0;
                 target_rad = arm_rad;
                 target_deg = arm_deg;
                 state++;
-            } else if (state == 3) {
+            } else if (state == 4) {
                 if(fabs(now_deg-target_deg) > 0.5) {      //現在角度と目標角度が0.5°以上離れている間回す
                     myled1 = 1;   //LED1 点灯
                     motor.AcOmega(target_rad);   //速度式角度制御
@@ -129,6 +142,7 @@
                     //wait(100);    //table停止用時間
                     servo_c.set_degree(0,servoDeg(deg_c_open));   //servo_c離す
                     myled4 = 0;
+//                    state = 0;
                 }
             }