File content as of revision 2:1ab1adf0915c:

#include "RS300.h"

RS300::RS300(PinName tx, PinName rx) : serial(tx, rx) {
    serial.baud(115200);
}

void RS300::send_packet(uint8_t adr, std::vector<uint8_t> &data, uint8_t cnt, uint8_t id, uint8_t flag)
{
    std::vector<uint8_t> buf;
    buf.push_back(0xFA);//Header :H
    buf.push_back(0xAF);//Header :L
    buf.push_back(id);  //id
    buf.push_back(flag);//flag
    buf.push_back(adr); //adress
    buf.push_back(data.size() / cnt);//data size

    buf.push_back(cnt);//amount of servo
    buf.insert(buf.end(), data.begin(), data.end());//data

    uint8_t sum = buf[2];//check sum from id to data
    for (int i = 3, size = buf.size(); i < size; ++i) {
        sum = sum ^ buf[i];
    }
    buf.push_back(sum);

    for (std::vector<uint8_t>::iterator i = buf.begin(); i != buf.end(); ++i) {
        serial.putc(*i);
    }
}

void RS300::on_all_servo()
{
    std::vector<uint8_t> dat;
    dat.push_back(0x01);
    for (uint8_t i = 1; i < 10; ++i) send_packet(0x24, dat, 1, i);//adress, on, servo amount, id(all)
}

void RS300::off_all_servo()
{
    std::vector<uint8_t> dat;
    dat.push_back(0x00);
    send_packet(0x24, dat, 1, 0xFF);//adress, off, servo amount, id(all)
}

void RS300::send_servo_pos(uint8_t id_s, std::vector<uint16_t> &pos)
{
    std::vector<uint8_t> buf;
    {
        uint8_t id = id_s;
        std::vector<uint16_t>::iterator i = pos.begin();
        for ( ; i != pos.end(); ++i, ++id) {
            buf.push_back(id);
            buf.push_back((uint8_t)((*i) & 0x00FF));
            buf.push_back((uint8_t)((*i) >> 8));
        }
    }
    send_packet(0x1E, buf, pos.size());
}