File content as of revision 24:2ab26ed08c83:

/**  
 * @file MX12.ccp  
 * @brief This file contains all the methods of the MX12 class 
 *        whose prototypes are in the MX12.h header file 
 */ 

#include "MX12.h"

MX12::MX12(PinName tx, PinName rx, int baud)
    : _mx12(tx, rx)    // initializes UnbufferedSerial object
{
    /*  Serial bus setup
     */
    // // Set desired properties (baud-8-N-1)
    _mx12.baud(baud);     /* modulation speed */
    _mx12.format(
        8,                /* bits */
        SerialBase::None, /* parity */
        1                 /* stop bit */
    ); 
    // Register a callback to process a Rx (receive) interrupt.
    _mx12.attach(callback(this, &MX12::_Rx_interrupt), SerialBase::RxIrq);
    
    // variable used for message reception
    _status_pck = {.raw = "",
                   .n_byte = 0,
                   .servo_id = 0, 
                   .length = 0,
                   .error = 0,
                   .n_param = 0,
                   .param = "", 
                   .received_checksum = 0,
                   .calculated_checksum = 0,
                   .parsed = false,
                   .valid = false};
                   
    _parser_state = {.expected_field = PacketField::Header1,
                       .byte_index = 0, 
                       .param_index = 0};
                       
    // Internal defaults states
    _bus_state = SerialState::Idle;
    
}

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;
    
    // Send instruction
    _bus_state = SerialState::Writing;
    rw(mot_id, CONTROL_TABLE_MOVING_SPEED, 2, data);
}

void MX12::SetSpeed_rad_s(unsigned char mot_id, float speed)
{
    if (speed > MX12_ABSOLUTE_MAX_SPEED_RAD_S) 
    {
        SetSpeed(mot_id, 1);
    } 
    else if (speed < -MX12_ABSOLUTE_MAX_SPEED_RAD_S) 
    {
        SetSpeed(mot_id, -1);
    }
    else 
    {
        SetSpeed(mot_id, speed / MX12_ABSOLUTE_MAX_SPEED_RAD_S);
    }
}

char MX12::IsAvailable(void) {
    return (_bus_state == SerialState::Idle);
}

void MX12::rw(unsigned char mot_id, char address, char len, char *data) {
    
    /* Set variables for reception from servovotor */
    _answer  = 0;
    _status_pck = {.raw = "",
                   .n_byte = 0,
                   .servo_id = 0, 
                   .length = 0,
                   .error = 0,
                   .n_param = 0,
                   .param = "", 
                   .received_checksum = 0,
                   .calculated_checksum = 0,
                   .parsed = false,
                   .valid = false};
    _parser_state = {.expected_field = PacketField::Header1,
                     .byte_index = 0, 
                     .param_index = 0};

    
    /* Initialise instruction packet to forge.
     * Instruction Packet is the command data sent to the servomotor.
     *
     * |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]...      |        |
     *                  \__  ___/                    \_  _/ \__  __/
     *                     \/           |              \/      \/            |
     *                   mot_id         |           address   data           |
     *                                  |                     (len = N-1)    |
     *                                  \__________________  ________________/
     *                                                     \/
     *                                                  Length ( cmd[3] )
     */
    char packet[16];
    unsigned char packet_length;
    
    /* Initialise checksum to calculate
     * It is used to check if packet is damaged during communication. 
     * Status Checksum is calculated according to the following formula:
     *
     * Status Checksum = ~( ID + Length + Error + Parameter1 + … Parameter N )
     */
    char checksum = 0x00;
        
    /* header 1 = 0xFF (dynamixel protocol 1.0) */
    packet[0] = 0xff;
    
    /* header 2 = 0xFF (dynamixel protocol 1.0) */
    packet[1] = 0xff; 
    
     /* packet ID i.e. servomotor id (dynamixel protocol 1.0) */
    packet[2] = mot_id;
    checksum += packet[2];
    
    /* 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:   1 INSTR +                               */
        /*       2 PARAM (starting address, length of data) + 1 CHKSUM     */
        packet[3] = 4;
        checksum += packet[3];

        /* set write instruction */
        packet[4] = PROTOCOL_INSTRUCTION_READ;
        checksum += packet[4];
        
        /* Param 1: address to read in the Control Table of RAM Area */
        packet[5] = address;
        checksum += packet[5];
        
        /* Param 2: number of bytes to read in the Control Table of RAM Area */
        packet[6] = len;
        checksum += packet[6];
        
        /* Checksum = ~( ID + Length + Instruction + Param1 + … Param N ) */
        packet[7] = ~checksum;
        
        packet_length = 8;
    }
    else // write instruction
    {
        /* byte length of the instruction: parameter and checksum field     */
        /* For write instruction:   1 INSTR +                               */
        /*       (1+len)PARAM (starting Address, bytes to write) + 1 CHKSUM */
        packet[3] = 3 + len; 
        checksum += packet[3];

        /* set read instruction */
        packet[4] = PROTOCOL_INSTRUCTION_WRITE;
        checksum += packet[4];

        /* Param 1: address to write in the "Control Table of RAM Area" */
        packet[5] = address;
        checksum += packet[5];
        
        /* Param 2 to N: data to write in the Control Table of RAM Area */
        for(char i = 0; i < len; i++) {
            packet[6 + i] = data[i];
            checksum += data[i];
        }
        
        /* Checksum = ~( ID + Length + Instruction + Param1 + … Param N ) */
        packet[6 + len] = ~checksum;
        
        packet_length = 7 + len;
    }
    
    // Send packet
    if(mot_id != 0xFE) {
        for(char i = 0; i < packet_length; i++) {
            _mx12.write(&packet[i], 1);
        }
    }
}

// Debug function to print Serial read
void MX12::PrintSerial() 
{
    for(int i = 0; i < _status_pck.n_byte; i++) {
        printf("%x ", _status_pck.raw[i]);
    }
    printf("\n");
}
 
MX12::Status MX12::GetStatus() {
    // Return the corresponding status code
    switch(_status_pck.error) {
        case 0:
            return Ok;
            break;
        case 1 << 0:
            return InputVoltageError;
            break;
        case 1 << 1:
            return AngleLimitError;
            break;
        case 1 << 2:
            return OverheatingError;
            break;
        case 1 << 3:
            return RangeError;
            break;
        case 1 << 4:
            return ChecksumError;
            break;
        case 1 << 5:
            return OverloadError;
            break;
        case 1 << 6:
            return InstructionError;
            break;
        default:
            return Unknown;
    }
}
 
void MX12::_Rx_interrupt() {

    char c;
    
    // Try to read serial
    if(_mx12.read(&c, 1)) {
        
        _status_pck.raw[(_parser_state.byte_index)++] = c;

        // State-machine parsing
        switch(_parser_state.expected_field) {
            
            /* c char is interpreted as a Header1 field */
            case PacketField::Header1:
            
                /* do nothing and set next state to Header2 */
                _parser_state.expected_field = PacketField::Header2;
                break;

            /* c char is interpreted as a Header2 field */
            case PacketField::Header2:
            
                /* do nothing and set next state to Id */
                _parser_state.expected_field = PacketField::Id;
                break;
                
            /* c char is interpreted as ID field */
            case PacketField::Id:
            
                /* store ID, update checksum and set next state to Length */
                _status_pck.servo_id = c;
                _status_pck.calculated_checksum += c; 
                _parser_state.expected_field = PacketField::Length;
                break;

            /* c char is interpreted as length of message data field
             * Length = number of Parameters + 2
             * where 2 stands for Length field (1 byte) + Error filed (1 byte)
             */
            case PacketField::Length:
            
                /* store number of param into _status_pck.n_param, 
                 * update calculated_checksum and set next state to Error 
                 */
                _status_pck.n_param = c - 2;
                _status_pck.calculated_checksum += c; 
                _parser_state.expected_field = PacketField::Error;
                break;
 
            /* c char is interpreted as error status field */
            case PacketField::Error:
            
                /* store error status, update checksum 
                 * and set next state to Data 
                 */
                _status_pck.error = c;
                _status_pck.calculated_checksum += c; 
                _parser_state.expected_field = PacketField::Data;
                break;
 
            /* c char is interpreted as a param field */
            case PacketField::Data:

                /* store current param, increase param_index 
                 * and update checksum */
                _status_pck.param[(_parser_state.param_index)++] = c;
                _status_pck.received_checksum += c; 
                
                /* increase param index (_parser_state.dataCount)
                 * and test if it is the last param to read
                 */
                if(_parser_state.param_index > _status_pck.n_param) {
                    /* reset param index and set next state to Checksum */
                    _parser_state.param_index = 0;
                    _parser_state.expected_field = PacketField::Checksum;
                }
                break;
            
            /* c char is interpreted as Checksum field */
            case PacketField::Checksum:
            
                /* store received_checksum, set parsed, store n_byte,
                 * evalutate valid and set next state to Header1 */
                _status_pck.received_checksum = c;
                _status_pck.parsed = true;
                _status_pck.n_byte = _parser_state.byte_index;
                _status_pck.valid = (_status_pck.received_checksum == c);
                _parser_state.expected_field = PacketField::Header1;
                
                /* set seriel state to Idle */
                _bus_state = SerialState::Idle;
                break;
                
            default:
                
                /* unexpected case. If it occurs it would be due to a 
                 * code error of this class  */
                break;
        }        
    }
}

/* Code from previous version of the class */

/*
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];
}
*/