File content as of revision 2:acaaf1126d76:

//mbed library
#include "mbed.h"
//ros library
#include "BufferedSerial.h"

#include <vector>
#include <cstdarg>
#include "LSCServo.h"

#define GET_LOW_char(A) (uint8_t)((A))
//Macro function  get lower 8 bits of A
#define GET_HIGH_char(A) (uint8_t)((A) >> 8)
//Macro function  get higher 8 bits of A
#define char_TO_HW(A, B) ((((uint16_t)(A)) << 8) | (uint8_t)(B))
//put A as higher 8 bits   B as lower 8 bits   which amalgamated into 16 bits integer

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)
{
}
//destructor
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];
    if(position < 0)
    position = 0;
    if(position > 1000)
    position = 1000;
    buf[0] = buf[1] = LOBOT_SERVO_FRAME_HEADER;
    buf[2] = 0x08;
    buf[3] = 0x03;
    buf[4] = 0x01;
    buf[5] = GET_LOW_char(time);
    buf[6] = GET_HIGH_char(time);
    buf[7] = GET_LOW_char(id);
    buf[8] = GET_LOW_char(position);
    buf[9] = GET_HIGH_char(position);
    serial.write(buf, 10);
}
void LSCServo::LobotSerialServoMove(vector<uint16_t> id, vector<int16_t> position, uint16_t time) //control list of serial servo move dedicated position
{
    char buf[126];
    int limit = id.size();
    int checksum = limit*3 + 6;
    buf[0] = buf[1] = LOBOT_SERVO_FRAME_HEADER;
    buf[2] = GET_LOW_char(checksum-2);
    buf[3] = 0x03;
    buf[4] = GET_LOW_char(limit);
    buf[5] = GET_LOW_char(time);
    buf[6] = GET_HIGH_char(time);
    int index = 7;
    for(int i = 0; i < limit; i++)
    {
        buf[index]=GET_LOW_char(id[i]);
        index++;
        if(position[i] < 0)
            position[i] = 0;
        if(position[i] > 1000)
            position[i] = 1000;
        buf[index]=GET_LOW_char(position[i]);
        index++;
        buf[index]=GET_HIGH_char(position[i]);
        index++;
    }
    serial.write(buf,checksum);
}
void LSCServo::LobotSerialActionGroupRun(uint16_t gpid, uint16_t count) //run action group(action group id, repeat time)
{    
    char buf[7];
    buf[0]=buf[1]=LOBOT_SERVO_FRAME_HEADER;
    buf[2]=0x05;
    buf[3]=0x06;
    buf[4]=GET_LOW_char(gpid);
    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%
{
    char buf[7];
    buf[0]=buf[1]=LOBOT_SERVO_FRAME_HEADER;
    buf[2]=0x05;
    buf[3]=0x0B;
    buf[4]=GET_LOW_char(gpid);
    buf[5]=GET_LOW_char(speed);
    buf[6]=GET_HIGH_char(speed);
    serial.write(buf,7);
}
int LSCServo::LobotSerialGetBatteryVoltage() //get voltage of board in terms of mV
{
    char buf[4];
    buf[0]=buf[1]=LOBOT_SERVO_FRAME_HEADER;
    buf[2]=0x02;
    buf[3]=0x0F;
    serial.write(buf,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
{
    char buf[4];
    buf[0]=buf[1]=LOBOT_SERVO_FRAME_HEADER;
    buf[2]=0x02;
    buf[3]=0x07;
    serial.write(buf,4);
}

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);
}