Diff: main.cpp

Revision:

5:131450b16ce3

Parent:

4:a59512fe0f9a

Child:

6:f1c0b9476c88

--- a/main.cpp	Mon Aug 27 15:36:28 2018 +0000
+++ b/main.cpp	Fri Aug 31 01:41:04 2018 +0000
@@ -36,7 +36,7 @@
 int i = 0;
 
 // PID
-PID motor_pid(7.2, 0, 0.13, Ts);// 6.4 0.13   7.2 0.13     8.4       6.5, 0, 0.19
+PID motor_pid(1500, 0, 40, Ts);// 6.4 0.13   7.2 0.13     8.4       6.5, 0, 0.19
 float PIDout = 0.0f;
 
 // Dynamixel
@@ -98,8 +98,8 @@
 
 void init_DYNAMIXEL()
 {
-    dynamixelClass.torqueMode(SERVO_ID, 1);
-//    dynamixelClass.setMode(SERVO_ID, WHEEL, 0, 0);
+//    dynamixelClass.torqueMode(SERVO_ID, 1);
+    dynamixelClass.setMode(SERVO_ID, WHEEL, 0, 0);
     wait_ms(1);
 }
 
@@ -121,8 +121,9 @@
     find_torque();
     motor_pid.Compute(torque_ref, torque_measured);
     PIDout = motor_pid.output;
-    servo_cmd = -PIDout*121.8f;   // 1023/8.4Nm = 121.7857
-
+//    servo_cmd = -PIDout*121.8f;   // 1023/8.4Nm = 121.7857
+    servo_cmd = -PIDout;   // 1023/8.4Nm = 121.7857
+/*
     // 電流控制
     if (servo_cmd > 0) {
         //                servo_cmd = servo_cmd + ((-torque_ref)*rate+friction)*121.8f;
@@ -141,11 +142,11 @@
     } else {
         row_cmd = servo_cmd;
     }
-    /*
+*/
     // 速度控制
         if (servo_cmd > 0) {
             row_cmd = servo_cmd;
-    //                servo_cmd = servo_cmd + ((-torque_ref)*rate+friction)*121.78f;
+//                    servo_cmd = servo_cmd + friction;
             servo_cmd = servo_cmd;
             rotation_ = 0;    // 0：Move Left
             if (servo_cmd >= 1023) {
@@ -154,7 +155,7 @@
             }
         } else {
             row_cmd = servo_cmd;
-    //                servo_cmd = -servo_cmd + ((torque_ref)*rate+friction)*121.78f;
+//                    servo_cmd = -servo_cmd - friction;
             servo_cmd = -servo_cmd;
             rotation_ = 1;    // 1：Move Right
             if (servo_cmd >= 1023) {
@@ -162,9 +163,9 @@
                 row_cmd = -servo_cmd;
             }
         }
-    */
-    dynamixelClass.torque(SERVO_ID, servo_cmd);
-//    dynamixelClass.wheel(SERVO_ID, rotation_, servo_cmd);  //0~1023   (rotation)
+    
+//    dynamixelClass.torque(SERVO_ID, servo_cmd);
+    dynamixelClass.wheel(SERVO_ID, rotation_, servo_cmd);  //0~1023   (rotation)
 //    dynamixelClass.wheel(SERVO_ID, 0, 150);  //0~1023      (rotation)
     uart_tx();
 }
@@ -185,7 +186,7 @@
 //    s3.j = servo_cmd;
 //    s4.j = 1;
 //    s5.j = 3;
-    s4.j = D_angle;
+    s4.j = D_Angle;
     s5.j = Angle*3;
     s6.j = row_cmd;
     s7.j = 1;