File content as of revision 1:1398d9ddd4ef:

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

#include <vector>
#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);
}