Chen Wei Ting / Mbed 2 deprecated zerotorque_final

zero torque and encoder

Dependencies:   MX28 PID mbed

Fork of LSM9DS1_project_5_zerotorque by Chen Wei Ting

Diff: main.cpp

Revision:
2:f630aff31d27
Parent:
1:2823a39f70a9
Child:
3:c05acc05c3bd
diff -r 2823a39f70a9 -r f630aff31d27 main.cpp
--- a/main.cpp	Tue Aug 07 08:47:59 2018 +0000
+++ b/main.cpp	Tue Aug 07 12:01:03 2018 +0000
@@ -116,7 +116,7 @@
                 row_cmd = servo_cmd;
             }
 
-//            dynamixelClass.torque(SERVO_ID, servo_cmd);
+            dynamixelClass.torque(SERVO_ID, servo_cmd);
             uart_tx();
             flag = 0;
         }
@@ -166,7 +166,7 @@
 //    s3.j = servo_cmd;
 //    s4.j = 1;
 //    s5.j = 3;
-    s4.j = angle_difference;
+    s4.j = (Angle*3-D_Angle)*360/4096;
     s5.j = torque_measured*1000;
     s6.j = row_cmd;
     s7.j = 1;