Diff: main.cpp

Revision:

8:9c3b291b3288

Parent:

7:5cb292622961

--- a/main.cpp	Sun Aug 12 15:24:02 2018 +0000
+++ b/main.cpp	Tue Aug 14 06:22:27 2018 +0000
@@ -58,7 +58,7 @@
 int i = 0;
 
 // PID
-PID motor_pid(15, 0, 0, Ts);// 6.4 0.13   7.2 0.13       4.8, 0.568, 0.142
+PID motor_pid(5, 0, 0, Ts);// 6.4 0.13   7.2 0.13       4.8, 0.568, 0.142
 float PIDout = 0.0f;
 
 // Dynamixel
@@ -221,8 +221,8 @@
 
     */
 //    dynamixelClass.torque(SERVO_ID, servo_cmd);
-    dynamixelClass.wheel(SERVO_ID, rotation_, servo_cmd);  //0~1023
-//        dynamixelClass.wheel(SERVO_ID, 0, 300);  //0~1023
+//    dynamixelClass.wheel(SERVO_ID, rotation_, servo_cmd);  //0~1023
+        dynamixelClass.wheel(SERVO_ID, 0, 300);  //0~1023
 //    wait_ms(1);
     uart_tx();
     flag = 0;
@@ -240,10 +240,11 @@
     splitter s6;
     splitter s7;
 
-    s1.j = D_Angle;
-    s2.j = Angle;
-//    s2.j = 1;
-    s3.j = Angle_Dif*360/4096;
+    s1.j = D_angle;
+//    s2.j = Angle;
+//    s3.j = Angle_Dif*360/4096;
+    s2.j = D_Angle;
+    s3.j = Angle;
     s4.j = _angle_difference*100;
     s5.j = torque_measured*100;
     s6.j = row_cmd;