Diff: main.cpp

Revision:

3:c05acc05c3bd

Parent:

2:f630aff31d27

Child:

4:a59512fe0f9a

--- a/main.cpp	Tue Aug 07 12:01:03 2018 +0000
+++ b/main.cpp	Fri Aug 24 02:47:02 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
+PID motor_pid(10, 0, 0, Ts);// 6.4 0.13   7.2 0.13
 float PIDout = 0.0f;
 
 // Dynamixel
@@ -91,41 +91,14 @@
     init_TIMER();
 
     while(1) {
-        if (flag==1) {
-            led2 = !led2;
-            angle_measure();
-            D_angle_measure();
-            find_torque();
-            motor_pid.Compute(torque_ref, torque_measured);
-            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;
-                if (servo_cmd >= 1023)
-                    servo_cmd = 1023;
-            } else {
-                servo_cmd = -servo_cmd + 1024 + ((torque_ref)*rate+friction)*121.8f;
-                if (servo_cmd >= 2047)
-                    servo_cmd = 2047;
-            }
-
-            if (servo_cmd >= 1023) {
-                row_cmd = -(servo_cmd-1023);
-            } else {
-                row_cmd = servo_cmd;
-            }
-
-            dynamixelClass.torque(SERVO_ID, servo_cmd);
-            uart_tx();
-            flag = 0;
-        }
     }
 }
 
 void init_DYNAMIXEL()
 {
     dynamixelClass.torqueMode(SERVO_ID, 1);
+//    dynamixelClass.setMode(SERVO_ID, WHEEL, 0, 0);
     wait_ms(1);
 }
 
@@ -141,13 +114,35 @@
 
 void timer1_ITR()
 {
-    flag = 1;
-//    if (led2f == 5) {
-//
-//        led2f = 0;
-//    } else {
-//        led2f++;
-//    }
+    led2 = !led2;
+    angle_measure();
+    D_angle_measure();
+    find_torque();
+    motor_pid.Compute(torque_ref, torque_measured);
+    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;
+        if (servo_cmd >= 1023)
+            servo_cmd = 1023;
+    } else {
+//                servo_cmd = -servo_cmd + 1024 + ((torque_ref)*rate+friction)*121.8f;
+        servo_cmd = -servo_cmd + 1024;
+        if (servo_cmd >= 2047)
+            servo_cmd = 2047;
+    }
+
+    if (servo_cmd >= 1023) {
+        row_cmd = -(servo_cmd-1023);
+    } else {
+        row_cmd = servo_cmd;
+    }
+
+    dynamixelClass.torque(SERVO_ID, servo_cmd);
+//    dynamixelClass.wheel(SERVO_ID, 0, 150);  //0~1023      (rotation)
+    uart_tx();
 }
 
 void uart_tx()
@@ -160,13 +155,13 @@
     splitter s6;
     splitter s7;
 
-    s1.j = 1;
-    s2.j = Angle;
-    s3.j = D_Angle;
+    s1.j = D_Angle;
+    s2.j = Angle*3;
+    s3.j = Angle_Dif*360/4096;
 //    s3.j = servo_cmd;
 //    s4.j = 1;
 //    s5.j = 3;
-    s4.j = (Angle*3-D_Angle)*360/4096;
+    s4.j = D_angle;
     s5.j = torque_measured*1000;
     s6.j = row_cmd;
     s7.j = 1;
@@ -208,6 +203,15 @@
         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;
     }
@@ -216,8 +220,8 @@
 void find_torque()
 {
 
-//    Angle_Dif = Angle*3-D_Angle;
-    angle_difference = (Angle*3-D_Angle)/4096.0f*2*PI;
+    Angle_Dif = Angle*3-D_Angle;
+    angle_difference = Angle_Dif/4096.0f*2*PI;
 //    angle_difference = Angle_Dif/4096.0f*2*PI;
 
     torque_measured = angle_difference*ks;