File content as of revision 0:35c27ebd9e8e:

/* 
Copyright (c) 2015 Jonathan Pickett (& Schakra, & Microsoft). Some appropriate open source license.
*/

#include <mbed.h>
#include "DynamixelBus.h"

/*****/

DynamixelBus::DynamixelBus( PinName txUartPin, PinName rxUartPin, PinName txSelectPin, PinName rxSelectPin, int baud )
    : _uart( txUartPin, rxUartPin), _txSelect(txSelectPin, 0), _rxSelect(rxSelectPin, 0) {
    _responseTimeout = .1;
    _baud = baud;
    _uart.baud(_baud);
    _replyDelay = .0012;
}

/*****/

StatusCode DynamixelBus::Ping( ServoId id )
{
    return send_ping( id );
}

/*****/

StatusCode  DynamixelBus::Read( ServoId id, unsigned char controlTableStart, unsigned char bytesToRead, CommBuffer& data )
{
    return send_read( id, controlTableStart, bytesToRead, data );
}

/*****/

StatusCode  DynamixelBus::Write( ServoId id, unsigned char controlTableStart, const CommBuffer& dataToWrite  )
{
    return send_write( id, controlTableStart, dataToWrite );
}

/*****/

StatusCode  DynamixelBus::WriteAction( ServoId id, unsigned char start, const CommBuffer& dataToWrite )
{
    return 0;
}

/*****/

void DynamixelBus::TriggerAction( ServoId id )
{
    return;
}

/*****/

StatusCode  DynamixelBus::HardReset( ServoId id )
{
    return 0;    
}

/*****/

StatusCode  DynamixelBus::SyncWrite( unsigned char controlTableStart, unsigned char bytesToWrite, vector<SyncIdDataPair>& data )
{
    return 0;
}

/*****/

unsigned char DynamixelBus::CalculateTxPacketChecksum( CommBuffer& packet )
{
    int sum = 0;
    for( int n = 0; n < packet.size(); n++ )
    {
        if( n == 0  || n == 1 )
        {
            continue;
        }
        
        sum += packet[n];   
    }

    return (~sum & 0xFF);   // AX-12 reference manual p. 10, Section 3-2
}

/*****/

// pings a servo at a specified ID
//
// returns the status code of the unit if found, otherwise returns 0
// 
// REVIEW: will Ping work if AX-12's StatusReturnLevel is set to not return status packets?
StatusCode DynamixelBus::send_ping( ServoId id )
{
    // 0xff, 0xff, ID, Length, Instruction(ping), Checksum
    CommBuffer txBuf(6);

    txBuf.push_back( 0xff );   
    txBuf.push_back( 0xff );
    txBuf.push_back( id );
    txBuf.push_back( 2 );       // length of remaining packet
    txBuf.push_back( instructionPing );
    txBuf.push_back( CalculateTxPacketChecksum( txBuf ) );

    _txSelect = 1;

    for( CommBuffer::iterator itSend = txBuf.begin(); itSend != txBuf.end(); itSend++)
    {
        _uart.putc(*itSend);
    }

    // Wait for data to transmit
    wait( (double)txBuf.size() * 8.0 * 1.0/_baud * 2.0 );

    // swap inputs for half duplex communication
    _txSelect = 0;
    _rxSelect = 1;

    wait( _replyDelay );

    // 0xff, 0xff, ID, Length, Error, Checksum
    CommBuffer rxBuf(6);

    // we'll only get a reply if it was not broadcast
    bool statusReceived = false;
    if( id != 0xFE )
    {
        Timer timeout;
        timeout.start();
        while( timeout.read() < _responseTimeout )                
        {
            if (_uart.readable())
            {
                rxBuf.push_back( _uart.getc() );
            }
            
            if( rxBuf.size() == 6)
            {
                statusReceived = true;
                break;
            }
        }
        timeout.stop();
    }
    
    _rxSelect = 0;

    if( statusReceived )
    {
        return (unsigned char)(rxBuf[4] & statusValid);
    }
    else
    {
        return 0;
    }
}

/*****/

StatusCode  DynamixelBus::send_read( ServoId id, unsigned char controlTableStart, unsigned char bytesToRead, CommBuffer& data )
{
    // 0xff, 0xff, ID, Packet Length, Instruction(read), Control Table Start, Length to Read, Checksum
    CommBuffer txBuf;

    txBuf.push_back( 0xff );   
    txBuf.push_back( 0xff );
    txBuf.push_back( id );
    txBuf.push_back( 4 );       // length of remaining packet
    txBuf.push_back( instructionRead );
    txBuf.push_back( controlTableStart );
    txBuf.push_back( bytesToRead );
    txBuf.push_back( CalculateTxPacketChecksum( txBuf ) );

    for( CommBuffer::iterator itSend = txBuf.begin(); itSend != txBuf.end(); itSend++)
    {
        _uart.putc(*itSend);
    }

    // Wait for data to transmit
    wait( (double)txBuf.size() * 8.0 * 1.0/_baud * 2.0 );
    _txSelect = 0;
    _rxSelect = 1;

    wait( _replyDelay );

    // 0xff, 0xff, ID, Length, Error, Data, Checksum
    int replySize = 6 + bytesToRead;
    CommBuffer replyBuf( replySize );

    // we'll only get a reply if it was not broadcast (and status reply option is selected in servo)
    bool statusReceived = false;
    if( id != 0xFE )
    {
        Timer timeout;
        timeout.start();
        while( timeout.read() < _responseTimeout )                
        {
            if (_uart.readable())
            {
                replyBuf.push_back( _uart.getc() );
            }
            
            if( replyBuf.size() == replySize )
            {
                statusReceived = true;
                break;
            }
        }
        timeout.stop();
    }
    
    _rxSelect = 0;

    if( statusReceived )
    {
        for( int n = 0; n < bytesToRead; n ++ )
        {
            data.push_back( replyBuf[5 + n] );
        }
        return replyBuf[4] & statusValid;
    }
    else
    {
        return 0;
    }
}

/*****/

StatusCode  DynamixelBus::send_write( ServoId id, unsigned char controlTableStart, const CommBuffer& dataToWrite )
{
    // 0xff, 0xff, ID, Packet Length, Instruction(write), Control Table Start, data to write, Checksum
    CommBuffer txBuf;

    // build packet
    txBuf.push_back( 0xff );   
    txBuf.push_back( 0xff );
    txBuf.push_back( id );
    txBuf.push_back( 3+dataToWrite.size() );       // length of remaining packet
    txBuf.push_back( instructionWrite );
    txBuf.push_back( controlTableStart );
    for( CommBuffer::const_iterator itD = dataToWrite.begin(); itD != dataToWrite.end(); itD++)
    {
        txBuf.push_back(*itD);
    }
    txBuf.push_back( CalculateTxPacketChecksum( txBuf ) );

//    printf( "sending: " );
    for( CommBuffer::iterator itSend = txBuf.begin(); itSend != txBuf.end(); itSend++)
    {
//        printf( "%02x ", *itSend);
    }
//    printf("\n");

    // enable transmission
    _txSelect = 1;

    for( CommBuffer::iterator itSend = txBuf.begin(); itSend != txBuf.end(); itSend++)
    {
        _uart.putc(*itSend);
    }

    // Wait for data to transmit
    double transmitDelay = (double)txBuf.size() * 8.0 * 1.0/(double)_baud;
    wait( transmitDelay );
    
    // switch inputs for half duplex operation
    _txSelect = 0;
    _rxSelect = 1;

    // 0xff, 0xff, ID, Length, Error, Data, Checksum
    CommBuffer replyBuf;

    // we'll only get a reply if it was not broadcast (TODO: and status reply option is selected in servo)
    bool statusReceived = false;
    Timer t;
    if( id != 0xFE )
    {
        t.start();

        while( t.read() < .002f )
        {
            if( _uart.readable())
            {
//                unsigned char b = _uart.getc();
// BUG with _uart.getc() on FRDM-K64F. Instead read directly from uart
                char b = UART3->D;
                
                replyBuf.push_back( b );
            
                if( replyBuf.size() == 6)
                {
                    statusReceived = true;
                    break;
                }
            }
        }
        
        t.stop();
    }

    _rxSelect = 0;

//    printf( "reply: ");

    for( CommBuffer::iterator itRepl = replyBuf.begin(); itRepl != replyBuf.end(); itRepl++)
    {
//        printf( "%02x ", *itRepl);
    }

//    printf("\n");
  

    if( statusReceived )
    {
        return replyBuf[4] & statusValid;
    }
    else
    {
        return 0;
    }
    
}