Diff: DynamixelBus.cpp

Revision:

0:35c27ebd9e8e

Child:

1:c5327c6d6239

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/DynamixelBus.cpp	Thu Dec 10 21:43:35 2015 +0000
@@ -0,0 +1,322 @@
+/* 
+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;
+    }
+    
+}