Telesensorics / Dynamixel

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependents:   RobotArmDemo

Fork of Dynamixel by robot arm demo team

DynamixelBus.cpp@0:35c27ebd9e8e, 2015-12-10 (annotated)

Committer:
jepickett
Date:
Thu Dec 10 21:43:35 2015 +0000
Revision:
0:35c27ebd9e8e
Child:
1:c5327c6d6239
initial dynamixel bus protocol implementation for FRDM-K64F. Lots of stuff left to flesh out.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jepickett 0:35c27ebd9e8e 1 /*
jepickett 0:35c27ebd9e8e 2 Copyright (c) 2015 Jonathan Pickett (& Schakra, & Microsoft). Some appropriate open source license.
jepickett 0:35c27ebd9e8e 3 */
jepickett 0:35c27ebd9e8e 4
jepickett 0:35c27ebd9e8e 5 #include <mbed.h>
jepickett 0:35c27ebd9e8e 6 #include "DynamixelBus.h"
jepickett 0:35c27ebd9e8e 7
jepickett 0:35c27ebd9e8e 8 /*****/
jepickett 0:35c27ebd9e8e 9
jepickett 0:35c27ebd9e8e 10 DynamixelBus::DynamixelBus( PinName txUartPin, PinName rxUartPin, PinName txSelectPin, PinName rxSelectPin, int baud )
jepickett 0:35c27ebd9e8e 11 : _uart( txUartPin, rxUartPin), _txSelect(txSelectPin, 0), _rxSelect(rxSelectPin, 0) {
jepickett 0:35c27ebd9e8e 12 _responseTimeout = .1;
jepickett 0:35c27ebd9e8e 13 _baud = baud;
jepickett 0:35c27ebd9e8e 14 _uart.baud(_baud);
jepickett 0:35c27ebd9e8e 15 _replyDelay = .0012;
jepickett 0:35c27ebd9e8e 16 }
jepickett 0:35c27ebd9e8e 17
jepickett 0:35c27ebd9e8e 18 /*****/
jepickett 0:35c27ebd9e8e 19
jepickett 0:35c27ebd9e8e 20 StatusCode DynamixelBus::Ping( ServoId id )
jepickett 0:35c27ebd9e8e 21 {
jepickett 0:35c27ebd9e8e 22 return send_ping( id );
jepickett 0:35c27ebd9e8e 23 }
jepickett 0:35c27ebd9e8e 24
jepickett 0:35c27ebd9e8e 25 /*****/
jepickett 0:35c27ebd9e8e 26
jepickett 0:35c27ebd9e8e 27 StatusCode DynamixelBus::Read( ServoId id, unsigned char controlTableStart, unsigned char bytesToRead, CommBuffer& data )
jepickett 0:35c27ebd9e8e 28 {
jepickett 0:35c27ebd9e8e 29 return send_read( id, controlTableStart, bytesToRead, data );
jepickett 0:35c27ebd9e8e 30 }
jepickett 0:35c27ebd9e8e 31
jepickett 0:35c27ebd9e8e 32 /*****/
jepickett 0:35c27ebd9e8e 33
jepickett 0:35c27ebd9e8e 34 StatusCode DynamixelBus::Write( ServoId id, unsigned char controlTableStart, const CommBuffer& dataToWrite )
jepickett 0:35c27ebd9e8e 35 {
jepickett 0:35c27ebd9e8e 36 return send_write( id, controlTableStart, dataToWrite );
jepickett 0:35c27ebd9e8e 37 }
jepickett 0:35c27ebd9e8e 38
jepickett 0:35c27ebd9e8e 39 /*****/
jepickett 0:35c27ebd9e8e 40
jepickett 0:35c27ebd9e8e 41 StatusCode DynamixelBus::WriteAction( ServoId id, unsigned char start, const CommBuffer& dataToWrite )
jepickett 0:35c27ebd9e8e 42 {
jepickett 0:35c27ebd9e8e 43 return 0;
jepickett 0:35c27ebd9e8e 44 }
jepickett 0:35c27ebd9e8e 45
jepickett 0:35c27ebd9e8e 46 /*****/
jepickett 0:35c27ebd9e8e 47
jepickett 0:35c27ebd9e8e 48 void DynamixelBus::TriggerAction( ServoId id )
jepickett 0:35c27ebd9e8e 49 {
jepickett 0:35c27ebd9e8e 50 return;
jepickett 0:35c27ebd9e8e 51 }
jepickett 0:35c27ebd9e8e 52
jepickett 0:35c27ebd9e8e 53 /*****/
jepickett 0:35c27ebd9e8e 54
jepickett 0:35c27ebd9e8e 55 StatusCode DynamixelBus::HardReset( ServoId id )
jepickett 0:35c27ebd9e8e 56 {
jepickett 0:35c27ebd9e8e 57 return 0;
jepickett 0:35c27ebd9e8e 58 }
jepickett 0:35c27ebd9e8e 59
jepickett 0:35c27ebd9e8e 60 /*****/
jepickett 0:35c27ebd9e8e 61
jepickett 0:35c27ebd9e8e 62 StatusCode DynamixelBus::SyncWrite( unsigned char controlTableStart, unsigned char bytesToWrite, vector<SyncIdDataPair>& data )
jepickett 0:35c27ebd9e8e 63 {
jepickett 0:35c27ebd9e8e 64 return 0;
jepickett 0:35c27ebd9e8e 65 }
jepickett 0:35c27ebd9e8e 66
jepickett 0:35c27ebd9e8e 67 /*****/
jepickett 0:35c27ebd9e8e 68
jepickett 0:35c27ebd9e8e 69 unsigned char DynamixelBus::CalculateTxPacketChecksum( CommBuffer& packet )
jepickett 0:35c27ebd9e8e 70 {
jepickett 0:35c27ebd9e8e 71 int sum = 0;
jepickett 0:35c27ebd9e8e 72 for( int n = 0; n < packet.size(); n++ )
jepickett 0:35c27ebd9e8e 73 {
jepickett 0:35c27ebd9e8e 74 if( n == 0 || n == 1 )
jepickett 0:35c27ebd9e8e 75 {
jepickett 0:35c27ebd9e8e 76 continue;
jepickett 0:35c27ebd9e8e 77 }
jepickett 0:35c27ebd9e8e 78
jepickett 0:35c27ebd9e8e 79 sum += packet[n];
jepickett 0:35c27ebd9e8e 80 }
jepickett 0:35c27ebd9e8e 81
jepickett 0:35c27ebd9e8e 82 return (~sum & 0xFF); // AX-12 reference manual p. 10, Section 3-2
jepickett 0:35c27ebd9e8e 83 }
jepickett 0:35c27ebd9e8e 84
jepickett 0:35c27ebd9e8e 85 /*****/
jepickett 0:35c27ebd9e8e 86
jepickett 0:35c27ebd9e8e 87 // pings a servo at a specified ID
jepickett 0:35c27ebd9e8e 88 //
jepickett 0:35c27ebd9e8e 89 // returns the status code of the unit if found, otherwise returns 0
jepickett 0:35c27ebd9e8e 90 //
jepickett 0:35c27ebd9e8e 91 // REVIEW: will Ping work if AX-12's StatusReturnLevel is set to not return status packets?
jepickett 0:35c27ebd9e8e 92 StatusCode DynamixelBus::send_ping( ServoId id )
jepickett 0:35c27ebd9e8e 93 {
jepickett 0:35c27ebd9e8e 94 // 0xff, 0xff, ID, Length, Instruction(ping), Checksum
jepickett 0:35c27ebd9e8e 95 CommBuffer txBuf(6);
jepickett 0:35c27ebd9e8e 96
jepickett 0:35c27ebd9e8e 97 txBuf.push_back( 0xff );
jepickett 0:35c27ebd9e8e 98 txBuf.push_back( 0xff );
jepickett 0:35c27ebd9e8e 99 txBuf.push_back( id );
jepickett 0:35c27ebd9e8e 100 txBuf.push_back( 2 ); // length of remaining packet
jepickett 0:35c27ebd9e8e 101 txBuf.push_back( instructionPing );
jepickett 0:35c27ebd9e8e 102 txBuf.push_back( CalculateTxPacketChecksum( txBuf ) );
jepickett 0:35c27ebd9e8e 103
jepickett 0:35c27ebd9e8e 104 _txSelect = 1;
jepickett 0:35c27ebd9e8e 105
jepickett 0:35c27ebd9e8e 106 for( CommBuffer::iterator itSend = txBuf.begin(); itSend != txBuf.end(); itSend++)
jepickett 0:35c27ebd9e8e 107 {
jepickett 0:35c27ebd9e8e 108 _uart.putc(*itSend);
jepickett 0:35c27ebd9e8e 109 }
jepickett 0:35c27ebd9e8e 110
jepickett 0:35c27ebd9e8e 111 // Wait for data to transmit
jepickett 0:35c27ebd9e8e 112 wait( (double)txBuf.size() * 8.0 * 1.0/_baud * 2.0 );
jepickett 0:35c27ebd9e8e 113
jepickett 0:35c27ebd9e8e 114 // swap inputs for half duplex communication
jepickett 0:35c27ebd9e8e 115 _txSelect = 0;
jepickett 0:35c27ebd9e8e 116 _rxSelect = 1;
jepickett 0:35c27ebd9e8e 117
jepickett 0:35c27ebd9e8e 118 wait( _replyDelay );
jepickett 0:35c27ebd9e8e 119
jepickett 0:35c27ebd9e8e 120 // 0xff, 0xff, ID, Length, Error, Checksum
jepickett 0:35c27ebd9e8e 121 CommBuffer rxBuf(6);
jepickett 0:35c27ebd9e8e 122
jepickett 0:35c27ebd9e8e 123 // we'll only get a reply if it was not broadcast
jepickett 0:35c27ebd9e8e 124 bool statusReceived = false;
jepickett 0:35c27ebd9e8e 125 if( id != 0xFE )
jepickett 0:35c27ebd9e8e 126 {
jepickett 0:35c27ebd9e8e 127 Timer timeout;
jepickett 0:35c27ebd9e8e 128 timeout.start();
jepickett 0:35c27ebd9e8e 129 while( timeout.read() < _responseTimeout )
jepickett 0:35c27ebd9e8e 130 {
jepickett 0:35c27ebd9e8e 131 if (_uart.readable())
jepickett 0:35c27ebd9e8e 132 {
jepickett 0:35c27ebd9e8e 133 rxBuf.push_back( _uart.getc() );
jepickett 0:35c27ebd9e8e 134 }
jepickett 0:35c27ebd9e8e 135
jepickett 0:35c27ebd9e8e 136 if( rxBuf.size() == 6)
jepickett 0:35c27ebd9e8e 137 {
jepickett 0:35c27ebd9e8e 138 statusReceived = true;
jepickett 0:35c27ebd9e8e 139 break;
jepickett 0:35c27ebd9e8e 140 }
jepickett 0:35c27ebd9e8e 141 }
jepickett 0:35c27ebd9e8e 142 timeout.stop();
jepickett 0:35c27ebd9e8e 143 }
jepickett 0:35c27ebd9e8e 144
jepickett 0:35c27ebd9e8e 145 _rxSelect = 0;
jepickett 0:35c27ebd9e8e 146
jepickett 0:35c27ebd9e8e 147 if( statusReceived )
jepickett 0:35c27ebd9e8e 148 {
jepickett 0:35c27ebd9e8e 149 return (unsigned char)(rxBuf[4] & statusValid);
jepickett 0:35c27ebd9e8e 150 }
jepickett 0:35c27ebd9e8e 151 else
jepickett 0:35c27ebd9e8e 152 {
jepickett 0:35c27ebd9e8e 153 return 0;
jepickett 0:35c27ebd9e8e 154 }
jepickett 0:35c27ebd9e8e 155 }
jepickett 0:35c27ebd9e8e 156
jepickett 0:35c27ebd9e8e 157 /*****/
jepickett 0:35c27ebd9e8e 158
jepickett 0:35c27ebd9e8e 159 StatusCode DynamixelBus::send_read( ServoId id, unsigned char controlTableStart, unsigned char bytesToRead, CommBuffer& data )
jepickett 0:35c27ebd9e8e 160 {
jepickett 0:35c27ebd9e8e 161 // 0xff, 0xff, ID, Packet Length, Instruction(read), Control Table Start, Length to Read, Checksum
jepickett 0:35c27ebd9e8e 162 CommBuffer txBuf;
jepickett 0:35c27ebd9e8e 163
jepickett 0:35c27ebd9e8e 164 txBuf.push_back( 0xff );
jepickett 0:35c27ebd9e8e 165 txBuf.push_back( 0xff );
jepickett 0:35c27ebd9e8e 166 txBuf.push_back( id );
jepickett 0:35c27ebd9e8e 167 txBuf.push_back( 4 ); // length of remaining packet
jepickett 0:35c27ebd9e8e 168 txBuf.push_back( instructionRead );
jepickett 0:35c27ebd9e8e 169 txBuf.push_back( controlTableStart );
jepickett 0:35c27ebd9e8e 170 txBuf.push_back( bytesToRead );
jepickett 0:35c27ebd9e8e 171 txBuf.push_back( CalculateTxPacketChecksum( txBuf ) );
jepickett 0:35c27ebd9e8e 172
jepickett 0:35c27ebd9e8e 173 for( CommBuffer::iterator itSend = txBuf.begin(); itSend != txBuf.end(); itSend++)
jepickett 0:35c27ebd9e8e 174 {
jepickett 0:35c27ebd9e8e 175 _uart.putc(*itSend);
jepickett 0:35c27ebd9e8e 176 }
jepickett 0:35c27ebd9e8e 177
jepickett 0:35c27ebd9e8e 178 // Wait for data to transmit
jepickett 0:35c27ebd9e8e 179 wait( (double)txBuf.size() * 8.0 * 1.0/_baud * 2.0 );
jepickett 0:35c27ebd9e8e 180 _txSelect = 0;
jepickett 0:35c27ebd9e8e 181 _rxSelect = 1;
jepickett 0:35c27ebd9e8e 182
jepickett 0:35c27ebd9e8e 183 wait( _replyDelay );
jepickett 0:35c27ebd9e8e 184
jepickett 0:35c27ebd9e8e 185 // 0xff, 0xff, ID, Length, Error, Data, Checksum
jepickett 0:35c27ebd9e8e 186 int replySize = 6 + bytesToRead;
jepickett 0:35c27ebd9e8e 187 CommBuffer replyBuf( replySize );
jepickett 0:35c27ebd9e8e 188
jepickett 0:35c27ebd9e8e 189 // we'll only get a reply if it was not broadcast (and status reply option is selected in servo)
jepickett 0:35c27ebd9e8e 190 bool statusReceived = false;
jepickett 0:35c27ebd9e8e 191 if( id != 0xFE )
jepickett 0:35c27ebd9e8e 192 {
jepickett 0:35c27ebd9e8e 193 Timer timeout;
jepickett 0:35c27ebd9e8e 194 timeout.start();
jepickett 0:35c27ebd9e8e 195 while( timeout.read() < _responseTimeout )
jepickett 0:35c27ebd9e8e 196 {
jepickett 0:35c27ebd9e8e 197 if (_uart.readable())
jepickett 0:35c27ebd9e8e 198 {
jepickett 0:35c27ebd9e8e 199 replyBuf.push_back( _uart.getc() );
jepickett 0:35c27ebd9e8e 200 }
jepickett 0:35c27ebd9e8e 201
jepickett 0:35c27ebd9e8e 202 if( replyBuf.size() == replySize )
jepickett 0:35c27ebd9e8e 203 {
jepickett 0:35c27ebd9e8e 204 statusReceived = true;
jepickett 0:35c27ebd9e8e 205 break;
jepickett 0:35c27ebd9e8e 206 }
jepickett 0:35c27ebd9e8e 207 }
jepickett 0:35c27ebd9e8e 208 timeout.stop();
jepickett 0:35c27ebd9e8e 209 }
jepickett 0:35c27ebd9e8e 210
jepickett 0:35c27ebd9e8e 211 _rxSelect = 0;
jepickett 0:35c27ebd9e8e 212
jepickett 0:35c27ebd9e8e 213 if( statusReceived )
jepickett 0:35c27ebd9e8e 214 {
jepickett 0:35c27ebd9e8e 215 for( int n = 0; n < bytesToRead; n ++ )
jepickett 0:35c27ebd9e8e 216 {
jepickett 0:35c27ebd9e8e 217 data.push_back( replyBuf[5 + n] );
jepickett 0:35c27ebd9e8e 218 }
jepickett 0:35c27ebd9e8e 219 return replyBuf[4] & statusValid;
jepickett 0:35c27ebd9e8e 220 }
jepickett 0:35c27ebd9e8e 221 else
jepickett 0:35c27ebd9e8e 222 {
jepickett 0:35c27ebd9e8e 223 return 0;
jepickett 0:35c27ebd9e8e 224 }
jepickett 0:35c27ebd9e8e 225 }
jepickett 0:35c27ebd9e8e 226
jepickett 0:35c27ebd9e8e 227 /*****/
jepickett 0:35c27ebd9e8e 228
jepickett 0:35c27ebd9e8e 229 StatusCode DynamixelBus::send_write( ServoId id, unsigned char controlTableStart, const CommBuffer& dataToWrite )
jepickett 0:35c27ebd9e8e 230 {
jepickett 0:35c27ebd9e8e 231 // 0xff, 0xff, ID, Packet Length, Instruction(write), Control Table Start, data to write, Checksum
jepickett 0:35c27ebd9e8e 232 CommBuffer txBuf;
jepickett 0:35c27ebd9e8e 233
jepickett 0:35c27ebd9e8e 234 // build packet
jepickett 0:35c27ebd9e8e 235 txBuf.push_back( 0xff );
jepickett 0:35c27ebd9e8e 236 txBuf.push_back( 0xff );
jepickett 0:35c27ebd9e8e 237 txBuf.push_back( id );
jepickett 0:35c27ebd9e8e 238 txBuf.push_back( 3+dataToWrite.size() ); // length of remaining packet
jepickett 0:35c27ebd9e8e 239 txBuf.push_back( instructionWrite );
jepickett 0:35c27ebd9e8e 240 txBuf.push_back( controlTableStart );
jepickett 0:35c27ebd9e8e 241 for( CommBuffer::const_iterator itD = dataToWrite.begin(); itD != dataToWrite.end(); itD++)
jepickett 0:35c27ebd9e8e 242 {
jepickett 0:35c27ebd9e8e 243 txBuf.push_back(*itD);
jepickett 0:35c27ebd9e8e 244 }
jepickett 0:35c27ebd9e8e 245 txBuf.push_back( CalculateTxPacketChecksum( txBuf ) );
jepickett 0:35c27ebd9e8e 246
jepickett 0:35c27ebd9e8e 247 // printf( "sending: " );
jepickett 0:35c27ebd9e8e 248 for( CommBuffer::iterator itSend = txBuf.begin(); itSend != txBuf.end(); itSend++)
jepickett 0:35c27ebd9e8e 249 {
jepickett 0:35c27ebd9e8e 250 // printf( "%02x ", *itSend);
jepickett 0:35c27ebd9e8e 251 }
jepickett 0:35c27ebd9e8e 252 // printf("\n");
jepickett 0:35c27ebd9e8e 253
jepickett 0:35c27ebd9e8e 254 // enable transmission
jepickett 0:35c27ebd9e8e 255 _txSelect = 1;
jepickett 0:35c27ebd9e8e 256
jepickett 0:35c27ebd9e8e 257 for( CommBuffer::iterator itSend = txBuf.begin(); itSend != txBuf.end(); itSend++)
jepickett 0:35c27ebd9e8e 258 {
jepickett 0:35c27ebd9e8e 259 _uart.putc(*itSend);
jepickett 0:35c27ebd9e8e 260 }
jepickett 0:35c27ebd9e8e 261
jepickett 0:35c27ebd9e8e 262 // Wait for data to transmit
jepickett 0:35c27ebd9e8e 263 double transmitDelay = (double)txBuf.size() * 8.0 * 1.0/(double)_baud;
jepickett 0:35c27ebd9e8e 264 wait( transmitDelay );
jepickett 0:35c27ebd9e8e 265
jepickett 0:35c27ebd9e8e 266 // switch inputs for half duplex operation
jepickett 0:35c27ebd9e8e 267 _txSelect = 0;
jepickett 0:35c27ebd9e8e 268 _rxSelect = 1;
jepickett 0:35c27ebd9e8e 269
jepickett 0:35c27ebd9e8e 270 // 0xff, 0xff, ID, Length, Error, Data, Checksum
jepickett 0:35c27ebd9e8e 271 CommBuffer replyBuf;
jepickett 0:35c27ebd9e8e 272
jepickett 0:35c27ebd9e8e 273 // we'll only get a reply if it was not broadcast (TODO: and status reply option is selected in servo)
jepickett 0:35c27ebd9e8e 274 bool statusReceived = false;
jepickett 0:35c27ebd9e8e 275 Timer t;
jepickett 0:35c27ebd9e8e 276 if( id != 0xFE )
jepickett 0:35c27ebd9e8e 277 {
jepickett 0:35c27ebd9e8e 278 t.start();
jepickett 0:35c27ebd9e8e 279
jepickett 0:35c27ebd9e8e 280 while( t.read() < .002f )
jepickett 0:35c27ebd9e8e 281 {
jepickett 0:35c27ebd9e8e 282 if( _uart.readable())
jepickett 0:35c27ebd9e8e 283 {
jepickett 0:35c27ebd9e8e 284 // unsigned char b = _uart.getc();
jepickett 0:35c27ebd9e8e 285 // BUG with _uart.getc() on FRDM-K64F. Instead read directly from uart
jepickett 0:35c27ebd9e8e 286 char b = UART3->D;
jepickett 0:35c27ebd9e8e 287
jepickett 0:35c27ebd9e8e 288 replyBuf.push_back( b );
jepickett 0:35c27ebd9e8e 289
jepickett 0:35c27ebd9e8e 290 if( replyBuf.size() == 6)
jepickett 0:35c27ebd9e8e 291 {
jepickett 0:35c27ebd9e8e 292 statusReceived = true;
jepickett 0:35c27ebd9e8e 293 break;
jepickett 0:35c27ebd9e8e 294 }
jepickett 0:35c27ebd9e8e 295 }
jepickett 0:35c27ebd9e8e 296 }
jepickett 0:35c27ebd9e8e 297
jepickett 0:35c27ebd9e8e 298 t.stop();
jepickett 0:35c27ebd9e8e 299 }
jepickett 0:35c27ebd9e8e 300
jepickett 0:35c27ebd9e8e 301 _rxSelect = 0;
jepickett 0:35c27ebd9e8e 302
jepickett 0:35c27ebd9e8e 303 // printf( "reply: ");
jepickett 0:35c27ebd9e8e 304
jepickett 0:35c27ebd9e8e 305 for( CommBuffer::iterator itRepl = replyBuf.begin(); itRepl != replyBuf.end(); itRepl++)
jepickett 0:35c27ebd9e8e 306 {
jepickett 0:35c27ebd9e8e 307 // printf( "%02x ", *itRepl);
jepickett 0:35c27ebd9e8e 308 }
jepickett 0:35c27ebd9e8e 309
jepickett 0:35c27ebd9e8e 310 // printf("\n");
jepickett 0:35c27ebd9e8e 311
jepickett 0:35c27ebd9e8e 312
jepickett 0:35c27ebd9e8e 313 if( statusReceived )
jepickett 0:35c27ebd9e8e 314 {
jepickett 0:35c27ebd9e8e 315 return replyBuf[4] & statusValid;
jepickett 0:35c27ebd9e8e 316 }
jepickett 0:35c27ebd9e8e 317 else
jepickett 0:35c27ebd9e8e 318 {
jepickett 0:35c27ebd9e8e 319 return 0;
jepickett 0:35c27ebd9e8e 320 }
jepickett 0:35c27ebd9e8e 321
jepickett 0:35c27ebd9e8e 322 }