File content as of revision 4:fe2a6b66cb85:

/* Copyright (C) 2016 Schakra Inc, MIT License
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy of this software
 * and associated documentation files (the "Software"), to deal in the Software without restriction,
 * including without limitation the rights to use, copy, modify, merge, publish, distribute,
 * sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all copies or
 * substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
 * BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
 * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 */
 
#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 = .004;
    _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;

    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
    double transmitDelay = (double)txBuf.size() * 8.0 * 1.0/(double)_baud;
    wait( transmitDelay );

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

    // 0xff, 0xff, ID, Length, Error, 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() < _responseTimeout )
        {
            if( _uart.readable())
            {
                // BUGBUG: 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;

    if( statusReceived )
    {
        return (unsigned char)(replyBuf[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 ) );

    _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 );

    _txSelect = 0;
    _rxSelect = 1;

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

    // we'll only get a reply if it was not broadcast (and status reply option is selected in servo)
    bool statusReceived = false;
    Timer t;
    if( id != 0xFE )
    {
        t.start();
        while( t.read() < _responseTimeout )                
        {
            if (_uart.readable())
            {
                // BUGBUG: 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() == replySize )
                {
                    statusReceived = true;
                    break;
                }
            }
        }
        t.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 ) );

    // 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() < _responseTimeout )
        {
            if( _uart.readable())
            {
                // BUGBUG: 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;

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