File content as of revision 3:4b96ab3e9e02:

#include "B3MServo.h"

//https://kondo-robot.com/faq/b3m_softwaremanual1200

/*sample program*********************************************

#include "mbed.h"
#include "B3MServo.h"

int main()
{
    B3MServo myServo(p13, p14, p12);
    myServo.onTorque(0);
    while(1)
    {
        myServo.controlAngle(0, 90);
        wait_ms(2500);
        myServo.controlAngle(0, -90);
        wait_ms(2500);
    }
}

************************************************************/

B3MServo::B3MServo(PinName tx, PinName rx, PinName enable) : BufferedSerial(tx, rx), _enable(enable)
{
    BufferedSerial::baud(115200);
    //Serial::baud(115200);
}

void B3MServo::sendPacket(char command, char id, char addr, int data, char size, char option, char count)
{
    char packet[0x09] = {};
    
    packet[0] = size+7;
    packet[1] = command;
    packet[2] = option;
    packet[3] = id;
    for(int i = 0; i < size; i++)
        packet[4+i] = char( ( data >> (i*8) ) & 0xFF );
    packet[size+4] = addr;
    packet[size+5] = count;
    packet[size+6] = 0;
    for(int i = 0; i < packet[0]-1; i++)
        packet[size+6] += packet[i];
        
    _enable = 1;
    BufferedSerial::write(packet, packet[0]);
    //送信待機時間が必要、短いと送信できないが、長いと受信できない
    wait_us(750);
    _enable = 0;
    wait_us(750);
    char re[5] = {};
    for(int i = 0; i < 5; i++)
    {
        re[i] = getc();
    }
}

void B3MServo::onTorque(char id, char movingMode, char controlMode)
{
    sendPacket(WRITE, id, TorqueON, int((controlMode << 2) | movingMode));
}
void B3MServo::controlAngle(char id, float angle)
{
    sendPacket(WRITE, id, DesiredPosition, int(angle*100));
}
void B3MServo::controlTorque(char id, int mNm)
{
    onTorque(id, Normal, Current);
    sendPacket(WRITE, id, DesiredTorque, mNm);
}

int B3MServo::readPacket(char id, char addr, char length)
{
    char packet[0x07] = {};
    
    packet[0] = 0x07;
    packet[1] = 0x03;
    packet[2] = 0x00;
    packet[3] = id;
    packet[4] = addr;
    packet[5] = length;
    packet[6] = 0;
    for(int i = 0; i < packet[0]-1; i++)
        packet[6] += packet[i];
    
    _enable = 1;
    BufferedSerial::write(packet, 7);
    wait_us(600);
    _enable = 0;
    wait_us(1000);
    char re[10] = {};
    for(int i = 0; i < 5+length; i++)
    {
        re[i] = getc();
    }
    char checksum = 0;
    for(int i = 0; i < 4+length; i++)
        checksum += re[i];
    if(checksum != re[4+length])
    {
        //printf("data error re%d != check%d", re[4+length], checksum);
        return;
    }
    
    int data = 0;
    for(int i = 0; i < length; i++)
    {
        data |= re[4+i] << (i*8);
    }
    return data;
}

float B3MServo::getCurrentPosition(char id)
{
    return float(readPacket(id, CurrentPosition, 2)) / 100;
}

float B3MServo::getCurrent(char id)
{
    return float(readPacket(id, CurrentAmpere, 2));
}