The function library of serial servo controller
Dependents: DR-ArmServoTest ros_button_2021 DR-ArmServoTest
Revision 2:acaaf1126d76, committed 2021-06-03
- Comitter:
- stivending
- Date:
- Thu Jun 03 04:56:29 2021 +0000
- Parent:
- 1:1398d9ddd4ef
- Commit message:
- add moveServos func
Changed in this revision
LSCServo.cpp | Show annotated file Show diff for this revision Revisions of this file |
LSCServo.h | Show annotated file Show diff for this revision Revisions of this file |
--- 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); +}
--- a/LSCServo.h Tue May 25 09:18:49 2021 +0000 +++ b/LSCServo.h Thu Jun 03 04:56:29 2021 +0000 @@ -25,5 +25,6 @@ void LobotSerialActionGroupSpeed(uint16_t id, uint16_t speed);//Set Run speed Action Group in XXX%, 0xFF for all int LobotSerialGetBatteryVoltage();//get voltage of board in terms of mV void LobotSerialActionGroupStop();//Stop current action group + void LSCServo::moveServos(uint8_t Num, uint16_t Time, ...); }; #endif \ No newline at end of file