Revision 6:fe45a1f21522, committed 2018-09-07

Comitter:

sgrsn

Date:

Fri Sep 07 02:59:41 2018 +0000

Parent:

4:5e8fad3a7991

Commit message:

Optimize wait time

Changed in this revision

diff -r 5e8fad3a7991 -r fe45a1f21522 B3MServo.cpp
--- 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)

diff -r 5e8fad3a7991 -r fe45a1f21522 B3MServo.h
--- a/B3MServo.h	Fri Sep 07 01:52:50 2018 +0000
+++ b/B3MServo.h	Fri Sep 07 02:59:41 2018 +0000
@@ -50,7 +50,7 @@
     
     private:
     DigitalOut _enable;
-    int control_mode;
+    char control_mode[0xFF];
 };
 
 #endif
\ No newline at end of file