Diff: B3MServo.cpp

Revision:

6:fe45a1f21522

Parent:

4:5e8fad3a7991

--- a/B3MServo.cpp	Fri Sep 07 01:52:50 2018 +0000
+++ b/B3MServo.cpp	Fri Sep 07 02:59:41 2018 +0000
@@ -25,7 +25,8 @@
 B3MServo::B3MServo(PinName tx, PinName rx, PinName enable) : BufferedSerial(tx, rx), _enable(enable)
 {
     BufferedSerial::baud(115200);
-    control_mode = 0xFF;
+    for(int i = 0; i < 0xFF; i++)
+    control_mode[i] = 0xFF;
     //Serial::baud(115200);
 }
 
@@ -85,17 +86,18 @@
 }
 void B3MServo::onTorque(char id, char movingMode, char controlMode)
 {
+    control_mode[id] = controlMode;
     sendPacket(WRITE, id, TorqueON, int((controlMode << 2) | movingMode));
 }
 void B3MServo::controlAngle(char id, float angle)
 {
-    if(control_mode == Position)
+    if(control_mode[id] == Position)
     {
         sendPacket(WRITE, id, DesiredPosition, int(angle*100));
+        wait_us(1000);
     }
     else
     {
-        control_mode = Position;
         onTorque(id, Normal, Position);
         wait_us(600);
         controlAngle(id, angle);
@@ -109,13 +111,13 @@
 }
 void B3MServo::controlTorque(char id, int mNm)
 {
-    if(control_mode == Current)
+    if(control_mode[id] == Current)
     {
         sendPacket(WRITE, id, DesiredTorque, mNm);
+        wait_us(1000);
     }
     else
     {
-        control_mode = Current;
         onTorque(id, Normal, Current);
         wait_us(600);
         controlTorque(id, mNm);
@@ -165,7 +167,9 @@
 
 float B3MServo::getCurrentPosition(char id)
 {
-    return float(readPacket(id, CurrentPosition, 2)) / 100;
+    int data = readPacket(id, CurrentPosition, 2);
+    wait_us(1000);
+    return float(data) / 100;
 }
 
 float B3MServo::getCurrent(char id)