Diff: LSCServo.cpp

Revision:

1:1398d9ddd4ef

Parent:

0:3b51e0118f2b

Child:

2:acaaf1126d76

--- a/LSCServo.cpp	Wed May 19 09:51:20 2021 +0000
+++ b/LSCServo.cpp	Tue May 25 09:18:49 2021 +0000
@@ -15,7 +15,6 @@
 
 using namespace std;
 
-
 //constructor
 LSCServo::LSCServo(PinName tx, PinName rx, uint32_t buf_size, uint32_t tx_multiple, const char* name) : serial(tx, rx, buf_size, tx_multiple)
 {
@@ -24,6 +23,20 @@
 LSCServo::~LSCServo()
 {
 }
+
+void LSCServo::receive_buffer(char* destination, int length)
+{
+    int index = 0;
+    while(index < length)
+    {
+        while(!serial.readable());
+        while(serial.readable())
+        {
+            destination[index++] = serial.getc();
+        }
+    }
+}
+
 void LSCServo::LobotSerialoneServoMove(uint16_t id, int16_t position, uint16_t time) //control one serial servo move dedicated position
 {
     char buf[10];
@@ -79,6 +92,9 @@
     buf[5]=GET_LOW_char(count);
     buf[6]=GET_HIGH_char(count);
     serial.write(buf,7);
+    
+    char word[7];
+    receive_buffer(word, 7);
 }
 void LSCServo::LobotSerialActionGroupSpeed(uint16_t gpid, uint16_t speed) //Set Run speed Action Group in XXX%
 {
@@ -98,18 +114,11 @@
     buf[2]=0x02;
     buf[3]=0x0F;
     serial.write(buf,4);
-    while(serial.readable()==0)
-    {
-        ;
-    }
-    MyBuffer<char> rx = serial.getc();
-    int limit = rx.getSize();
-    char* word;
-    for(int i = 0; i < limit; i++)
-    {
-        *(word + i) = rx;
-    }
-    int voltage = char_TO_HW(*(word + 5), *(word + 4));
+    
+    char word[6];
+    receive_buffer(word, 6);
+    
+    int voltage = char_TO_HW(word[5], word[4]);
     return voltage;
 }
 void LSCServo::LobotSerialActionGroupStop() //Stop current action group