hidaka sato / B3MServo

Library for Kondo B3M Servo Motor

Dependencies:   BufferedSerial

Diff: B3MServo.cpp

Revision:
5:3ed0a70f697b
Parent:
4:5e8fad3a7991
diff -r 5e8fad3a7991 -r 3ed0a70f697b B3MServo.cpp
--- a/B3MServo.cpp	Fri Sep 07 01:52:50 2018 +0000
+++ b/B3MServo.cpp	Fri Sep 07 01:58:54 2018 +0000
@@ -85,6 +85,7 @@
 }
 void B3MServo::onTorque(char id, char movingMode, char controlMode)
 {
+    control_mode = controlMode;
     sendPacket(WRITE, id, TorqueON, int((controlMode << 2) | movingMode));
 }
 void B3MServo::controlAngle(char id, float angle)
@@ -95,7 +96,6 @@
     }
     else
     {
-        control_mode = Position;
         onTorque(id, Normal, Position);
         wait_us(600);
         controlAngle(id, angle);
@@ -115,7 +115,6 @@
     }
     else
     {
-        control_mode = Current;
         onTorque(id, Normal, Current);
         wait_us(600);
         controlTorque(id, mNm);