Diff: main.cpp

Revision:

7:5cb292622961

Parent:

6:f48c51662e27

Child:

8:9c3b291b3288

--- a/main.cpp	Sat Aug 11 04:58:46 2018 +0000
+++ b/main.cpp	Sun Aug 12 15:24:02 2018 +0000
@@ -58,7 +58,7 @@
 int i = 0;
 
 // PID
-PID motor_pid(100, 0, 0, Ts);// 6.4 0.13   7.2 0.13       4.8, 0.568, 0.142
+PID motor_pid(15, 0, 0, Ts);// 6.4 0.13   7.2 0.13       4.8, 0.568, 0.142
 float PIDout = 0.0f;
 
 // Dynamixel
@@ -75,9 +75,9 @@
 double torque_measured = 0.0;
 float ks = 2.6393*2;  //spring constant
 //int angle_dif = 0;
-float torque_ref = 0;
-//float friction = 0.0f;
+float torque_ref = 0.0f; //*************************************************
 float friction = 0.0f;
+//float friction = 0.5f;
 float rate = 0.8;
 //float friction = 0.0f;
 //float check = 0.0f;
@@ -151,26 +151,29 @@
     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*121.78f;   //  1023/8.4Nm = 121.78
 
     if (servo_cmd > 0) {
-//                servo_cmd = servo_cmd + ((-torque_ref)*rate+friction)*22.73f;
-        servo_cmd = servo_cmd;
-         rotation_ = 0;// 0：Move Left 
-        if (servo_cmd >= 1023)
+        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 {
-//                servo_cmd = -servo_cmd + 1024 + ((torque_ref)*rate+friction)*22.73f;
-//        servo_cmd = -servo_cmd+1024;
-        servo_cmd = -servo_cmd;
-        rotation_ = 1;// 1：Move Right 
-//        if (servo_cmd >= 2047)
-//            servo_cmd = 2047;
-        if (servo_cmd >= 1023)
+        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;
+        }
     }
 
-    row_cmd = servo_cmd;
+//    row_cmd = servo_cmd;
 
 //    if (servo_cmd > 1023) {
 //        row_cmd = -(servo_cmd-1023);
@@ -219,7 +222,8 @@
     */
 //    dynamixelClass.torque(SERVO_ID, servo_cmd);
     dynamixelClass.wheel(SERVO_ID, rotation_, servo_cmd);  //0~1023
-    wait_ms(1);
+//        dynamixelClass.wheel(SERVO_ID, 0, 300);  //0~1023
+//    wait_ms(1);
     uart_tx();
     flag = 0;
     wait_ms(1);
@@ -236,13 +240,13 @@
     splitter s6;
     splitter s7;
 
-    s1.j = _angle_difference*100;
-    s2.j = row_cmd;
+    s1.j = D_Angle;
+    s2.j = Angle;
 //    s2.j = 1;
-    s3.j = servo_cmd;
-    s4.j = D_Angle;
-    s5.j = D_angle;
-    s6.j = Angle;
+    s3.j = Angle_Dif*360/4096;
+    s4.j = _angle_difference*100;
+    s5.j = torque_measured*100;
+    s6.j = row_cmd;
     s7.j = 1;
 
     T[2] = s1.C[0];
@@ -281,9 +285,20 @@
         D_angle_old = D_angle;
         d++;
     } else {
+
         D_angle_dif = D_angle - D_angle_old;
+
+        if (D_angle_dif > 4096/2) {
+            D_angle_dif = -(4096-(D_angle_dif));
+        } else if (D_angle_dif < -4096/2) {
+            D_angle_dif = -(-4096-(D_angle_dif));
+        } else {
+            D_angle_dif = D_angle_dif;
+        }
+
         D_Angle = D_Angle + D_angle_dif;
         D_angle_old = D_angle;
+
     }
 }
 
@@ -301,12 +316,12 @@
 //    if (_angle_difference > 99) {
 //        _angle_difference = _angle_difference-100.54;
 //    }
-        if (_angle_difference > 6) {
-        _angle_difference = _angle_difference-6.4;
-    }
-    if (_angle_difference < -6) {
-        _angle_difference = _angle_difference-6.4;
-    }
+    /*        if (_angle_difference > 6) {
+                _angle_difference = _angle_difference-6.4;
+            }
+            if (_angle_difference < -6) {
+                _angle_difference = _angle_difference-6.4;
+            }*/
     torque_measured = _angle_difference*ks;
 //    torque_measured = Angle_Dif;
 }