Diff: main.cpp

Revision:

4:a59512fe0f9a

Parent:

3:c05acc05c3bd

Child:

5:131450b16ce3

--- a/main.cpp	Fri Aug 24 02:47:02 2018 +0000
+++ b/main.cpp	Mon Aug 27 15:36:28 2018 +0000
@@ -36,7 +36,7 @@
 int i = 0;
 
 // PID
-PID motor_pid(10, 0, 0, Ts);// 6.4 0.13   7.2 0.13
+PID motor_pid(7.2, 0, 0.13, Ts);// 6.4 0.13   7.2 0.13     8.4       6.5, 0, 0.19
 float PIDout = 0.0f;
 
 // Dynamixel
@@ -60,6 +60,7 @@
 //float friction = 0.0f;
 //float check = 0.0f;
 float Angle_Dif;
+short rotation_;
 
 // function
 void init_UART();
@@ -122,13 +123,14 @@
     PIDout = motor_pid.output;
     servo_cmd = -PIDout*121.8f;   // 1023/8.4Nm = 121.7857
 
+    // 電流控制
     if (servo_cmd > 0) {
-//                servo_cmd = servo_cmd + ((-torque_ref)*rate+friction)*121.8f;
+        //                servo_cmd = servo_cmd + ((-torque_ref)*rate+friction)*121.8f;
         servo_cmd = servo_cmd;
         if (servo_cmd >= 1023)
             servo_cmd = 1023;
     } else {
-//                servo_cmd = -servo_cmd + 1024 + ((torque_ref)*rate+friction)*121.8f;
+        //                servo_cmd = -servo_cmd + 1024 + ((torque_ref)*rate+friction)*121.8f;
         servo_cmd = -servo_cmd + 1024;
         if (servo_cmd >= 2047)
             servo_cmd = 2047;
@@ -139,8 +141,30 @@
     } 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;
+            rotation_ = 0;    // 0：Move Left
+            if (servo_cmd >= 1023) {
+                servo_cmd = 1023;
+                row_cmd = servo_cmd;
+            }
+        } else {
+            row_cmd = servo_cmd;
+    //                servo_cmd = -servo_cmd + ((torque_ref)*rate+friction)*121.78f;
+            servo_cmd = -servo_cmd;
+            rotation_ = 1;    // 1：Move Right
+            if (servo_cmd >= 1023) {
+                servo_cmd = 1023;
+                row_cmd = -servo_cmd;
+            }
+        }
+    */
     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();
 }
@@ -155,14 +179,14 @@
     splitter s6;
     splitter s7;
 
-    s1.j = D_Angle;
-    s2.j = Angle*3;
-    s3.j = Angle_Dif*360/4096;
+    s1.j = torque_ref*1000;
+    s2.j = torque_measured*1000;
+    s3.j = Angle_Dif/4096*3600;
 //    s3.j = servo_cmd;
 //    s4.j = 1;
 //    s5.j = 3;
     s4.j = D_angle;
-    s5.j = torque_measured*1000;
+    s5.j = Angle*3;
     s6.j = row_cmd;
     s7.j = 1;