File content as of revision 2:02f3323a107d:

#include "MX12.h"

MX12::MX12(PinName tx, PinName rx, int baud) : _mx12(tx, rx) {
    _baud = baud;
    _mx12.baud(_baud);
    _mx12.format(8, SerialBase::None, 1);
    _mx12.attach(callback(this, &MX12::_ReadCallback), SerialBase::RxIrq);
    _state = State::Available;
}

void MX12::SetSpeed(unsigned char mot_id, float speed) {
    char data[2];
 
    // Speed absolute value
    int goal = (0x3ff * abs(speed));
 
    // Spin direction (CW is negative)
    if (speed < 0) {
        goal |= (0x1 << 10);
    }

    data[0] = goal & 0xff;
    data[1] = goal >> 8;
    
    // Enter writing state
    _state = State::Writing;
 
    // Send instruction
    rw(mot_id, 0x20, 2, data);
}

char MX12::IsAvailable(void) {
    return _state == State::Available;
}

void MX12::ReadPosition(unsigned char mot_id) {
    // Make a request, interrupt takes care of everything else
    _state = State::ReadingPosition;
    rw(mot_id, 0x24, 2, NULL);
}

float MX12::GetPosition(unsigned char mot_id) {
    return _angle[mot_id];
}

MX12::Status MX12::GetStatus(void) {
    return _status;
}

// Debug function to print Serial read
void MX12::PrintAnswer() {
    for(char i = 0; i < MX12_ANSWER_MAX_SIZE; i++) {
        printf("%x ", _res[i]);
    }
    
    printf("\r\n");
}

void MX12::rw(unsigned char mot_id, char address, char len, char *data) {
    _res_count = 0;
    memset(_res, 0, MX12_ANSWER_MAX_SIZE);
    
    /* Forge instruction packet */
    char cmd[16];
    
    /* Instruction Packet is the command data sent to the Device.
     *
     * |Header1|Header2|Packet ID|Length|Instruction|Param1...ParamN|Checksum |
     * |-------|-------|---------|------|-----------|---------------|---------|
     * |  0xFF |  0xFF |Packet ID|Length|Instruction|Param1...ParamN| CHKSUM  |
     * | cmd[0]| cmd[1]|  cmd[2] |cmd[3]|  cmd[4]   |cmd[5]...      |(2 bytes)|
     *                                                     \__  ___/
     *                                              |         \/              |
     *                                              |        len = N-1        |
     *                                              \____________  ___________/
     *                                                           \/
     *                                                         Length (cmd[4])
     */
    
    /* header 1 = 0xFF (dynamixel protocol 1.0) */
    cmd[0] = 0xff;
    
    /* header 2 = 0xFF (dynamixel protocol 1.0) */
    cmd[1] = 0xff; 
    
     /* packet ID i.e. servomotor id (dynamixel protocol 1.0) */
    cmd[2] = mot_id;

    /* Length indicates the byte size of the instruction, 
       parameter and Checksum field */
       
    
    /* Guess instruction type. NULL for read, not NULL for write */
    if(data == NULL) // read instruction
    {
        /* byte length of the instruction: parameter and checksum field. */
        /* for read instruction:                                         */
        /*         2 PARAM (starting address, length of data) + 2 CHKSUM */
        cmd[3] = 4; // 

        /* set write instruction */
        cmd[4] = PROTOCOL_INSTRUCTION_READ;
        
        /* Param 1: address to read in the "Control Table of RAM Area" */
        cmd[5] = address;
    }
    else // write instruction
    {
        /* byte length of the instruction: parameter and checksum field     */
        /* For write instruction:                                           */
        /*    (1 + len) PARAM (starting Address, bytes to write) + 2 CHKSUM */
        cmd[3] = 3 + len; 

        /* set read instruction */
        cmd[4] = PROTOCOL_INSTRUCTION_WRITE;

        /* Param 1: address to write in the "Control Table of RAM Area" */
        cmd[5] = address;
    }
    
    //cmd[4] = 0x02 + (data != NULL); // original code from Titouan
    
    
    // Compute checksum
    if(data == NULL) {
        cmd[6] = len;
        cmd[7] = 0xFF - (mot_id + 4 + 2 + address + len);
        
        // [XXX] Force length to one to force send
        len = 1;
    } else {
        char cs = mot_id + len + address + 6;
        
        for(char i = 0; i < len; i++) {
            cmd[6 + i] = data[i];
            cs += data[i];
        }
        
        cmd[6 + len] = 0xFF - cs;
    }
    
    // Send packet
    if(mot_id != 0xFE) {
        for(char i = 0; i < (7 + len); i++) {
            _mx12.write(&cmd[i], 1);
        }
    }
}

void MX12::_ReadCallback() {
    char c;
    
    // Loop on reading serial
    if(_mx12.read(&c, 1)) {
        _res[_res_count] = c;
        _res_count++;
        if(_res_count >= MX12_ANSWER_MAX_SIZE) _res_count = 0;
        
        // Find answer in buffer
        char ans_i = 2;
        for(; (_res[ans_i] != 0xFF) && (ans_i <= MX12_ANSWER_MAX_SIZE - 1); ans_i++);
        if(ans_i >= MX12_ANSWER_MAX_SIZE) return;
        
        ans_i += 2;
        char mot_id = _res[ans_i++];
        char len = _res[ans_i++];
        _chksm = _res[ans_i + len - 1];
        
        // [TODO] Verify checksum
        if(len != 0 && _chksm != 0) {
            // Interpret answer depending on state
            switch(_state) {
                case State::ReadingPosition:
                    _angle[mot_id] = (((uint16_t) _res[ans_i + 1] << 8) | (uint16_t) _res[ans_i]) * 0.088;
                    _state = State::Available;
                    break;
                case State::Writing:
                    // Return the corresponding status code
                    switch(_res[ans_i]) {
                        case 0:
                            _status = Status::Ok;
                            break;
                        case 1 << 0:
                            _status = Status::InputVoltageError;
                            break;
                        case 1 << 1:
                            _status = Status::AngleLimitError;
                            break;
                        case 1 << 2:
                            _status = Status::OverheatingError;
                            break;
                        case 1 << 3:
                            _status = Status::RangeError;
                            break;
                        case 1 << 4:
                            _status = Status::ChecksumError;
                            break;
                        case 1 << 5:
                            _status = Status::OverloadError;
                            break;
                        case 1 << 6:
                            _status = Status::InstructionError;
                            break;
                        default:
                            _status = Status::Unknown;
                    }

                    _state = State::Available;
                    break;
                default:
                    _status = Status::Unknown;
            }
        }
    }
}