Diff: main.cpp

Revision:

6:f48c51662e27

Parent:

5:4dbed091ec5a

Child:

7:5cb292622961

--- a/main.cpp	Fri Aug 10 18:09:00 2018 +0000
+++ b/main.cpp	Sat Aug 11 04:58:46 2018 +0000
@@ -58,7 +58,7 @@
 int i = 0;
 
 // PID
-PID motor_pid(5, 0, 0, Ts);// 6.4 0.13   7.2 0.13       4.8, 0.568, 0.142
+PID motor_pid(100, 0, 0, Ts);// 6.4 0.13   7.2 0.13       4.8, 0.568, 0.142
 float PIDout = 0.0f;
 
 // Dynamixel
@@ -151,7 +151,7 @@
     find_torque();
     motor_pid.Compute(torque_ref, torque_measured);
     PIDout = motor_pid.output;
-    servo_cmd = -PIDout*22.73f;   // 1023/45rpm = 22.73
+    servo_cmd = PIDout*22.73f;   // 1023/45rpm = 22.73
 
     if (servo_cmd > 0) {
 //                servo_cmd = servo_cmd + ((-torque_ref)*rate+friction)*22.73f;
@@ -236,7 +236,7 @@
     splitter s6;
     splitter s7;
 
-    s1.j = Angle_Dif;
+    s1.j = _angle_difference*100;
     s2.j = row_cmd;
 //    s2.j = 1;
     s3.j = servo_cmd;
@@ -295,11 +295,17 @@
 
     _angle_difference = Angle_Dif/4096*2*_PI;
 //    if ((_angle_difference < 0) && (D_angle < 0)) {
-    if (_angle_difference < -99) {
-        _angle_difference = _angle_difference-100.54;
+//    if (_angle_difference < -99) {
+//        _angle_difference = _angle_difference-100.54;
+//    }
+//    if (_angle_difference > 99) {
+//        _angle_difference = _angle_difference-100.54;
+//    }
+        if (_angle_difference > 6) {
+        _angle_difference = _angle_difference-6.4;
     }
-    if (_angle_difference > 99) {
-        _angle_difference = _angle_difference-100.54;
+    if (_angle_difference < -6) {
+        _angle_difference = _angle_difference-6.4;
     }
     torque_measured = _angle_difference*ks;
 //    torque_measured = Angle_Dif;