Diff: LSCServo.cpp

Revision:

2:acaaf1126d76

Parent:

1:1398d9ddd4ef

--- a/LSCServo.cpp	Tue May 25 09:18:49 2021 +0000
+++ b/LSCServo.cpp	Thu Jun 03 04:56:29 2021 +0000
@@ -4,6 +4,7 @@
 #include "BufferedSerial.h"
 
 #include <vector>
+#include <cstdarg>
 #include "LSCServo.h"
 
 #define GET_LOW_char(A) (uint8_t)((A))
@@ -128,4 +129,33 @@
     buf[2]=0x02;
     buf[3]=0x07;
     serial.write(buf,4);
-}
\ No newline at end of file
+}
+
+void LSCServo::moveServos(uint8_t Num, uint16_t Time, ...)
+{
+    uint8_t buf[128];
+    va_list arg_ptr;
+    va_start(arg_ptr, Time);
+    if (Num < 1 || Num > 32 || (!(Time > 0))) {
+        return; 
+    }
+    buf[0] = LOBOT_SERVO_FRAME_HEADER;  
+    buf[1] = LOBOT_SERVO_FRAME_HEADER;
+    buf[2] = Num * 3 + 5;   
+    buf[3] = 0x03;  
+    buf[4] = Num;    
+    buf[5] = GET_LOW_char(Time);
+    buf[6] = GET_HIGH_char(Time); 
+    uint8_t index = 7;
+    for (uint8_t i = 0; i < Num; i++) {
+        uint16_t tmp = va_arg(arg_ptr, uint16_t);
+        buf[index++] = GET_LOW_char(tmp);
+                                         
+        uint16_t pos = va_arg(arg_ptr, uint16_t);
+        buf[index++] = GET_LOW_char(pos);
+        buf[index++] = GET_HIGH_char(pos);
+    }
+    va_end(arg_ptr);  
+    serial.write(buf, buf[2] + 2); 
+    wait_ms(Time);
+}