DynamixelBus.cpp

00001 /* Copyright (C) 2016 Telesensorics Inc, MIT License
00002  *
00003  * Permission is hereby granted, free of charge, to any person obtaining a copy of this software
00004  * and associated documentation files (the "Software"), to deal in the Software without restriction,
00005  * including without limitation the rights to use, copy, modify, merge, publish, distribute,
00006  * sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is
00007  * furnished to do so, subject to the following conditions:
00008  *
00009  * The above copyright notice and this permission notice shall be included in all copies or
00010  * substantial portions of the Software.
00011  *
00012  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
00013  * BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
00014  * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
00015  * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
00016  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
00017  */
00018  
00019 #include <mbed.h>
00020 #include "DynamixelBus.h"
00021 
00022 
00023 DynamixelBus::DynamixelBus( PinName txUartPin, PinName rxUartPin, PinName txSelectPin, PinName rxSelectPin, int baud )
00024     : _uart( txUartPin, rxUartPin), _txSelect(txSelectPin, 0), _rxSelect(rxSelectPin, 0) {
00025     _responseTimeout = .004;
00026     _baud = baud;
00027     _uart.baud(_baud);
00028     _replyDelay = .0012;
00029 }
00030 
00031 
00032 StatusCode DynamixelBus::Ping( ServoId id )
00033 {
00034     return send_ping( id );
00035 }
00036 
00037 
00038 StatusCode  DynamixelBus::Read( ServoId id, unsigned char controlTableStart, unsigned char bytesToRead, CommBuffer& data )
00039 {
00040     return send_read( id, controlTableStart, bytesToRead, data );
00041 }
00042 
00043 
00044 StatusCode  DynamixelBus::Write( ServoId id, unsigned char controlTableStart, const CommBuffer& dataToWrite  )
00045 {
00046     return send_write( id, controlTableStart, dataToWrite );
00047 }
00048 
00049 
00050 StatusCode  DynamixelBus::WriteAction( ServoId id, unsigned char start, const CommBuffer& dataToWrite )
00051 {
00052     return 0;
00053 }
00054 
00055 
00056 void DynamixelBus::TriggerAction( ServoId id )
00057 {
00058     return;
00059 }
00060 
00061 
00062 StatusCode  DynamixelBus::HardReset( ServoId id )
00063 {
00064     return 0;    
00065 }
00066 
00067 
00068 StatusCode  DynamixelBus::SyncWrite( unsigned char controlTableStart, unsigned char bytesToWrite, vector<SyncIdDataPair>& data )
00069 {
00070     return 0;
00071 }
00072 
00073 
00074 unsigned char DynamixelBus::CalculateTxPacketChecksum( CommBuffer& packet )
00075 {
00076     int sum = 0;
00077     for( int n = 0; n < packet.size(); n++ )
00078     {
00079         if( n == 0  || n == 1 )
00080         {
00081             continue;
00082         }
00083         
00084         sum += packet[n];   
00085     }
00086 
00087     return (~sum & 0xFF);   // AX-12 reference manual p. 10, Section 3-2
00088 }
00089 
00090 
00091 // pings a servo at a specified ID
00092 //
00093 // returns the status code of the unit if found, otherwise returns 0
00094 // 
00095 // REVIEW: will Ping work if AX-12's StatusReturnLevel is set to not return status packets?
00096 StatusCode DynamixelBus::send_ping( ServoId id )
00097 {
00098     // 0xff, 0xff, ID, Length, Instruction(ping), Checksum
00099     CommBuffer txBuf;
00100 
00101     txBuf.push_back( 0xff );   
00102     txBuf.push_back( 0xff );
00103     txBuf.push_back( id );
00104     txBuf.push_back( 2 );       // length of remaining packet
00105     txBuf.push_back( instructionPing );
00106     txBuf.push_back( CalculateTxPacketChecksum( txBuf ) );
00107 
00108     _txSelect = 1;
00109 
00110     for( CommBuffer::iterator itSend = txBuf.begin(); itSend != txBuf.end(); itSend++)
00111     {
00112         _uart.putc(*itSend);
00113     }
00114 
00115     // Wait for data to transmit
00116     double transmitDelay = (double)txBuf.size() * 8.0 * 1.0/(double)_baud;
00117     wait( transmitDelay );
00118 
00119     // swap inputs for half duplex communication
00120     _txSelect = 0;
00121     _rxSelect = 1;
00122 
00123     // 0xff, 0xff, ID, Length, Error, Checksum
00124     CommBuffer replyBuf;
00125 
00126     // we'll only get a reply if it was not broadcast (TODO: and status reply option is selected in servo)
00127     bool statusReceived = false;
00128     Timer t;
00129     if( id != 0xFE )
00130     {
00131         t.start();
00132 
00133         while( t.read() < _responseTimeout )
00134         {
00135             if( _uart.readable())
00136             {
00137                 // BUGBUG: unsigned char b = _uart.getc();
00138                 // Bug with _uart.getc() on FRDM-K64F. Instead read directly from uart
00139                 char b = UART3->D;
00140                 
00141                 replyBuf.push_back( b );
00142             
00143                 if( replyBuf.size() == 6)
00144                 {
00145                     statusReceived = true;
00146                     break;
00147                 }
00148             }
00149         }
00150         
00151         t.stop();
00152     }
00153 
00154     _rxSelect = 0;
00155 
00156     if( statusReceived )
00157     {
00158         return (unsigned char)(replyBuf[4] | statusValid);
00159     }
00160     else
00161     {
00162         return 0;
00163     }
00164 }
00165 
00166 
00167 StatusCode  DynamixelBus::send_read( ServoId id, unsigned char controlTableStart, unsigned char bytesToRead, CommBuffer& data )
00168 {
00169     // 0xff, 0xff, ID, Packet Length, Instruction(read), Control Table Start, Length to Read, Checksum
00170     CommBuffer txBuf;
00171 
00172     txBuf.push_back( 0xff );   
00173     txBuf.push_back( 0xff );
00174     txBuf.push_back( id );
00175     txBuf.push_back( 4 );       // length of remaining packet
00176     txBuf.push_back( instructionRead );
00177     txBuf.push_back( controlTableStart );
00178     txBuf.push_back( bytesToRead );
00179     txBuf.push_back( CalculateTxPacketChecksum( txBuf ) );
00180 
00181     _txSelect = 1;
00182 
00183     for( CommBuffer::iterator itSend = txBuf.begin(); itSend != txBuf.end(); itSend++)
00184     {
00185         _uart.putc(*itSend);
00186     }
00187 
00188     // Wait for data to transmit
00189     double transmitDelay = (double)txBuf.size() * 8.0 * 1.0/(double)_baud;
00190     wait( transmitDelay );
00191 
00192     _txSelect = 0;
00193     _rxSelect = 1;
00194 
00195     // 0xff, 0xff, ID, Length, Error, Data, Checksum
00196     int replySize = 6 + bytesToRead;
00197     CommBuffer replyBuf;
00198 
00199     // we'll only get a reply if it was not broadcast (and status reply option is selected in servo)
00200     bool statusReceived = false;
00201     Timer t;
00202     if( id != 0xFE )
00203     {
00204         t.start();
00205         while( t.read() < _responseTimeout )                
00206         {
00207             if (_uart.readable())
00208             {
00209                 // BUGBUG: unsigned char b = _uart.getc();
00210                 // Bug with _uart.getc() on FRDM-K64F. Instead read directly from uart
00211                 char b = UART3->D;
00212                 replyBuf.push_back( b );
00213 
00214                 if( replyBuf.size() == replySize )
00215                 {
00216                     statusReceived = true;
00217                     break;
00218                 }
00219             }
00220         }
00221         t.stop();
00222     }
00223     
00224     _rxSelect = 0;
00225 
00226     if( statusReceived )
00227     {
00228         for( int n = 0; n < bytesToRead; n ++ )
00229         {
00230             data.push_back( replyBuf[5 + n] );
00231         }
00232         return replyBuf[4] | statusValid;
00233     }
00234     else
00235     {
00236         return 0;
00237     }
00238 }
00239 
00240 
00241 StatusCode  DynamixelBus::send_write( ServoId id, unsigned char controlTableStart, const CommBuffer& dataToWrite )
00242 {
00243     // 0xff, 0xff, ID, Packet Length, Instruction(write), Control Table Start, data to write, Checksum
00244     CommBuffer txBuf;
00245 
00246     // build packet
00247     txBuf.push_back( 0xff );   
00248     txBuf.push_back( 0xff );
00249     txBuf.push_back( id );
00250     txBuf.push_back( 3+dataToWrite.size() );       // length of remaining packet
00251     txBuf.push_back( instructionWrite );
00252     txBuf.push_back( controlTableStart );
00253     for( CommBuffer::const_iterator itD = dataToWrite.begin(); itD != dataToWrite.end(); itD++)
00254     {
00255         txBuf.push_back(*itD);
00256     }
00257     txBuf.push_back( CalculateTxPacketChecksum( txBuf ) );
00258 
00259     // enable transmission
00260     _txSelect = 1;
00261 
00262     for( CommBuffer::iterator itSend = txBuf.begin(); itSend != txBuf.end(); itSend++)
00263     {
00264         _uart.putc(*itSend);
00265     }
00266 
00267     // Wait for data to transmit
00268     double transmitDelay = (double)txBuf.size() * 8.0 * 1.0/(double)_baud;
00269     wait( transmitDelay );
00270     
00271     // switch inputs for half duplex operation
00272     _txSelect = 0;
00273     _rxSelect = 1;
00274 
00275     // 0xff, 0xff, ID, Length, Error, Data, Checksum
00276     CommBuffer replyBuf;
00277 
00278     // we'll only get a reply if it was not broadcast (TODO: and status reply option is selected in servo)
00279     bool statusReceived = false;
00280     Timer t;
00281     if( id != 0xFE )
00282     {
00283         t.start();
00284 
00285         while( t.read() < _responseTimeout )
00286         {
00287             if( _uart.readable())
00288             {
00289                 // BUGBUG: unsigned char b = _uart.getc();
00290                 // Bug with _uart.getc() on FRDM-K64F. Instead read directly from uart
00291                 char b = UART3->D;
00292                 replyBuf.push_back( b );
00293             
00294                 if( replyBuf.size() == 6)
00295                 {
00296                     statusReceived = true;
00297                     break;
00298                 }
00299             }
00300         }
00301         
00302         t.stop();
00303     }
00304 
00305     _rxSelect = 0;
00306 
00307     if( statusReceived )
00308     {
00309         return replyBuf[4] | statusValid;
00310     }
00311     else
00312     {
00313         return 0;
00314     }
00315     
00316 }