dynamixel protocol engine
Fork of Dynamixel by
DynamixelBus.cpp
- Committer:
- henryrawas
- Date:
- 2016-01-19
- Revision:
- 3:37aa8024931e
- Parent:
- 2:536f775454c3
- Child:
- 4:fe2a6b66cb85
File content as of revision 3:37aa8024931e:
/* 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 = .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; } }