Diff: main.cpp

Revision:

3:78b608ba0286

Parent:

2:b728a6564520

Child:

4:3abefa13c2e3

--- a/main.cpp	Thu Jul 08 12:55:18 2021 +0000
+++ b/main.cpp	Thu Jul 08 18:20:31 2021 +0000
@@ -22,7 +22,7 @@
 DigitalOut myled1(LED1);
 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_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計算
 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)
@@ -75,17 +75,17 @@
 
     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);
+//    target_rad = -(M_PI/6); //目標角度をπ/6（rad）に（単位に注意）           //////////////////////////
+//    target_deg = radToDeg(target_rad);
 
     while (1) {
         ////    ROS     ////
         nh.spinOnce();
         if(arm_num==1&&arm_deg==30) {       //LEDは動作確認用
             myled2 = !myled2;
-            myled3 = !myled3;
             arm_rad = degToRad(arm_deg);
-//            target_rad = arm_rad;
+            target_rad = -30;
+            target_deg = radToDeg(target_rad);
         }
 
         ////タッチセンサ////
@@ -95,16 +95,24 @@
                 servo_c.set_degree(0,servoDeg(10000));   //servo_c掴む
                 hand = 1;
             }
+        } else {
+            hand = 0;
         }
 
         ////アブソリュートエンコーダ////
         if(timer.read()>DELTA_T) {
             if(fabs(now_deg-target_deg) > 0.5) {      //現在角度と目標角度が0.5°以上離れている間回す
-                myled1=1;   //LED2 点灯
-                motor.AcOmega(target_rad);   //速度式角度制御
+                myled1=1;   //LED1 点灯
+                if(hand == 1) {
+                    myled4=1;   //LED4 点灯
+                    motor.AcOmega(target_rad);   //速度式角度制御
+                } else if(hand == 0) {
+                    myled4=0;
+                }
             } else { //目標角度に達したら停止
-                myled1=0;   //LED2 消灯
+                myled1=0;   //LED1 消灯
                 motor.stop();
+                myled4=0;   //LED4 点灯
                 //wait(100);    //table停止用時間
                 servo_c.set_degree(0,servoDeg(7500));   //servo_c離す
             }