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@3:37aa8024931e, 2016-01-19 (annotated)

Committer:
henryrawas
Date:
Tue Jan 19 19:53:15 2016 +0000
Revision:
3:37aa8024931e
Parent:
2:536f775454c3
Child:
4:fe2a6b66cb85
Roll back checkin of debug code

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 1:c5327c6d6239 12 _responseTimeout = .004;
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 1:c5327c6d6239 95 CommBuffer txBuf;
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 1:c5327c6d6239 112 double transmitDelay = (double)txBuf.size() * 8.0 * 1.0/(double)_baud;
jepickett 1:c5327c6d6239 113 wait( transmitDelay );
jepickett 0:35c27ebd9e8e 114
jepickett 0:35c27ebd9e8e 115 // swap inputs for half duplex communication
jepickett 0:35c27ebd9e8e 116 _txSelect = 0;
jepickett 0:35c27ebd9e8e 117 _rxSelect = 1;
jepickett 0:35c27ebd9e8e 118
jepickett 1:c5327c6d6239 119 // 0xff, 0xff, ID, Length, Error, Checksum
jepickett 1:c5327c6d6239 120 CommBuffer replyBuf;
jepickett 0:35c27ebd9e8e 121
jepickett 1:c5327c6d6239 122 // we'll only get a reply if it was not broadcast (TODO: and status reply option is selected in servo)
jepickett 0:35c27ebd9e8e 123 bool statusReceived = false;
jepickett 1:c5327c6d6239 124 Timer t;
jepickett 0:35c27ebd9e8e 125 if( id != 0xFE )
jepickett 0:35c27ebd9e8e 126 {
jepickett 1:c5327c6d6239 127 t.start();
jepickett 1:c5327c6d6239 128
jepickett 1:c5327c6d6239 129 while( t.read() < _responseTimeout )
jepickett 0:35c27ebd9e8e 130 {
jepickett 1:c5327c6d6239 131 if( _uart.readable())
jepickett 0:35c27ebd9e8e 132 {
jepickett 1:c5327c6d6239 133 // BUGBUG: unsigned char b = _uart.getc();
jepickett 1:c5327c6d6239 134 // Bug with _uart.getc() on FRDM-K64F. Instead read directly from uart
jepickett 1:c5327c6d6239 135 char b = UART3->D;
jepickett 1:c5327c6d6239 136
jepickett 1:c5327c6d6239 137 replyBuf.push_back( b );
jepickett 0:35c27ebd9e8e 138
jepickett 1:c5327c6d6239 139 if( replyBuf.size() == 6)
jepickett 1:c5327c6d6239 140 {
jepickett 1:c5327c6d6239 141 statusReceived = true;
jepickett 1:c5327c6d6239 142 break;
jepickett 1:c5327c6d6239 143 }
jepickett 0:35c27ebd9e8e 144 }
jepickett 0:35c27ebd9e8e 145 }
jepickett 1:c5327c6d6239 146
jepickett 1:c5327c6d6239 147 t.stop();
jepickett 0:35c27ebd9e8e 148 }
jepickett 1:c5327c6d6239 149
jepickett 0:35c27ebd9e8e 150 _rxSelect = 0;
jepickett 0:35c27ebd9e8e 151
jepickett 0:35c27ebd9e8e 152 if( statusReceived )
jepickett 0:35c27ebd9e8e 153 {
jepickett 1:c5327c6d6239 154 return (unsigned char)(replyBuf[4] | statusValid);
jepickett 0:35c27ebd9e8e 155 }
jepickett 0:35c27ebd9e8e 156 else
jepickett 0:35c27ebd9e8e 157 {
jepickett 0:35c27ebd9e8e 158 return 0;
jepickett 0:35c27ebd9e8e 159 }
jepickett 0:35c27ebd9e8e 160 }
jepickett 0:35c27ebd9e8e 161
jepickett 0:35c27ebd9e8e 162 /*****/
jepickett 0:35c27ebd9e8e 163
jepickett 0:35c27ebd9e8e 164 StatusCode DynamixelBus::send_read( ServoId id, unsigned char controlTableStart, unsigned char bytesToRead, CommBuffer& data )
jepickett 0:35c27ebd9e8e 165 {
jepickett 0:35c27ebd9e8e 166 // 0xff, 0xff, ID, Packet Length, Instruction(read), Control Table Start, Length to Read, Checksum
jepickett 0:35c27ebd9e8e 167 CommBuffer txBuf;
jepickett 0:35c27ebd9e8e 168
jepickett 0:35c27ebd9e8e 169 txBuf.push_back( 0xff );
jepickett 0:35c27ebd9e8e 170 txBuf.push_back( 0xff );
jepickett 0:35c27ebd9e8e 171 txBuf.push_back( id );
jepickett 0:35c27ebd9e8e 172 txBuf.push_back( 4 ); // length of remaining packet
jepickett 0:35c27ebd9e8e 173 txBuf.push_back( instructionRead );
jepickett 0:35c27ebd9e8e 174 txBuf.push_back( controlTableStart );
jepickett 0:35c27ebd9e8e 175 txBuf.push_back( bytesToRead );
jepickett 0:35c27ebd9e8e 176 txBuf.push_back( CalculateTxPacketChecksum( txBuf ) );
jepickett 0:35c27ebd9e8e 177
jepickett 1:c5327c6d6239 178 _txSelect = 1;
jepickett 1:c5327c6d6239 179
jepickett 0:35c27ebd9e8e 180 for( CommBuffer::iterator itSend = txBuf.begin(); itSend != txBuf.end(); itSend++)
jepickett 0:35c27ebd9e8e 181 {
jepickett 0:35c27ebd9e8e 182 _uart.putc(*itSend);
jepickett 0:35c27ebd9e8e 183 }
jepickett 0:35c27ebd9e8e 184
jepickett 0:35c27ebd9e8e 185 // Wait for data to transmit
jepickett 1:c5327c6d6239 186 double transmitDelay = (double)txBuf.size() * 8.0 * 1.0/(double)_baud;
jepickett 1:c5327c6d6239 187 wait( transmitDelay );
jepickett 1:c5327c6d6239 188
jepickett 0:35c27ebd9e8e 189 _txSelect = 0;
jepickett 0:35c27ebd9e8e 190 _rxSelect = 1;
jepickett 0:35c27ebd9e8e 191
jepickett 0:35c27ebd9e8e 192 // 0xff, 0xff, ID, Length, Error, Data, Checksum
jepickett 0:35c27ebd9e8e 193 int replySize = 6 + bytesToRead;
jepickett 1:c5327c6d6239 194 CommBuffer replyBuf;
jepickett 0:35c27ebd9e8e 195
jepickett 0:35c27ebd9e8e 196 // we'll only get a reply if it was not broadcast (and status reply option is selected in servo)
jepickett 0:35c27ebd9e8e 197 bool statusReceived = false;
jepickett 1:c5327c6d6239 198 Timer t;
jepickett 0:35c27ebd9e8e 199 if( id != 0xFE )
jepickett 0:35c27ebd9e8e 200 {
jepickett 1:c5327c6d6239 201 t.start();
jepickett 1:c5327c6d6239 202 while( t.read() < _responseTimeout )
jepickett 0:35c27ebd9e8e 203 {
jepickett 0:35c27ebd9e8e 204 if (_uart.readable())
jepickett 0:35c27ebd9e8e 205 {
jepickett 1:c5327c6d6239 206 // BUGBUG: unsigned char b = _uart.getc();
jepickett 1:c5327c6d6239 207 // Bug with _uart.getc() on FRDM-K64F. Instead read directly from uart
jepickett 1:c5327c6d6239 208 char b = UART3->D;
jepickett 1:c5327c6d6239 209 replyBuf.push_back( b );
jepickett 1:c5327c6d6239 210
jepickett 1:c5327c6d6239 211 if( replyBuf.size() == replySize )
jepickett 1:c5327c6d6239 212 {
jepickett 1:c5327c6d6239 213 statusReceived = true;
jepickett 1:c5327c6d6239 214 break;
jepickett 1:c5327c6d6239 215 }
jepickett 0:35c27ebd9e8e 216 }
jepickett 0:35c27ebd9e8e 217 }
jepickett 1:c5327c6d6239 218 t.stop();
jepickett 0:35c27ebd9e8e 219 }
jepickett 0:35c27ebd9e8e 220
jepickett 0:35c27ebd9e8e 221 _rxSelect = 0;
jepickett 0:35c27ebd9e8e 222
jepickett 0:35c27ebd9e8e 223 if( statusReceived )
jepickett 0:35c27ebd9e8e 224 {
jepickett 0:35c27ebd9e8e 225 for( int n = 0; n < bytesToRead; n ++ )
jepickett 0:35c27ebd9e8e 226 {
jepickett 0:35c27ebd9e8e 227 data.push_back( replyBuf[5 + n] );
jepickett 0:35c27ebd9e8e 228 }
jepickett 1:c5327c6d6239 229 return replyBuf[4] | statusValid;
jepickett 0:35c27ebd9e8e 230 }
jepickett 0:35c27ebd9e8e 231 else
jepickett 0:35c27ebd9e8e 232 {
jepickett 0:35c27ebd9e8e 233 return 0;
jepickett 0:35c27ebd9e8e 234 }
jepickett 0:35c27ebd9e8e 235 }
jepickett 0:35c27ebd9e8e 236
jepickett 0:35c27ebd9e8e 237 /*****/
jepickett 0:35c27ebd9e8e 238
jepickett 0:35c27ebd9e8e 239 StatusCode DynamixelBus::send_write( ServoId id, unsigned char controlTableStart, const CommBuffer& dataToWrite )
jepickett 0:35c27ebd9e8e 240 {
jepickett 0:35c27ebd9e8e 241 // 0xff, 0xff, ID, Packet Length, Instruction(write), Control Table Start, data to write, Checksum
jepickett 0:35c27ebd9e8e 242 CommBuffer txBuf;
jepickett 0:35c27ebd9e8e 243
jepickett 0:35c27ebd9e8e 244 // build packet
jepickett 0:35c27ebd9e8e 245 txBuf.push_back( 0xff );
jepickett 0:35c27ebd9e8e 246 txBuf.push_back( 0xff );
jepickett 0:35c27ebd9e8e 247 txBuf.push_back( id );
jepickett 0:35c27ebd9e8e 248 txBuf.push_back( 3+dataToWrite.size() ); // length of remaining packet
jepickett 0:35c27ebd9e8e 249 txBuf.push_back( instructionWrite );
jepickett 0:35c27ebd9e8e 250 txBuf.push_back( controlTableStart );
jepickett 0:35c27ebd9e8e 251 for( CommBuffer::const_iterator itD = dataToWrite.begin(); itD != dataToWrite.end(); itD++)
jepickett 0:35c27ebd9e8e 252 {
jepickett 0:35c27ebd9e8e 253 txBuf.push_back(*itD);
jepickett 0:35c27ebd9e8e 254 }
jepickett 0:35c27ebd9e8e 255 txBuf.push_back( CalculateTxPacketChecksum( txBuf ) );
jepickett 0:35c27ebd9e8e 256
jepickett 0:35c27ebd9e8e 257 // enable transmission
jepickett 0:35c27ebd9e8e 258 _txSelect = 1;
jepickett 0:35c27ebd9e8e 259
jepickett 0:35c27ebd9e8e 260 for( CommBuffer::iterator itSend = txBuf.begin(); itSend != txBuf.end(); itSend++)
jepickett 0:35c27ebd9e8e 261 {
jepickett 0:35c27ebd9e8e 262 _uart.putc(*itSend);
jepickett 0:35c27ebd9e8e 263 }
jepickett 0:35c27ebd9e8e 264
jepickett 0:35c27ebd9e8e 265 // Wait for data to transmit
jepickett 0:35c27ebd9e8e 266 double transmitDelay = (double)txBuf.size() * 8.0 * 1.0/(double)_baud;
jepickett 0:35c27ebd9e8e 267 wait( transmitDelay );
jepickett 0:35c27ebd9e8e 268
jepickett 0:35c27ebd9e8e 269 // switch inputs for half duplex operation
jepickett 0:35c27ebd9e8e 270 _txSelect = 0;
jepickett 0:35c27ebd9e8e 271 _rxSelect = 1;
jepickett 0:35c27ebd9e8e 272
jepickett 0:35c27ebd9e8e 273 // 0xff, 0xff, ID, Length, Error, Data, Checksum
jepickett 0:35c27ebd9e8e 274 CommBuffer replyBuf;
jepickett 0:35c27ebd9e8e 275
jepickett 0:35c27ebd9e8e 276 // we'll only get a reply if it was not broadcast (TODO: and status reply option is selected in servo)
jepickett 0:35c27ebd9e8e 277 bool statusReceived = false;
jepickett 0:35c27ebd9e8e 278 Timer t;
jepickett 0:35c27ebd9e8e 279 if( id != 0xFE )
jepickett 0:35c27ebd9e8e 280 {
jepickett 0:35c27ebd9e8e 281 t.start();
jepickett 0:35c27ebd9e8e 282
jepickett 1:c5327c6d6239 283 while( t.read() < _responseTimeout )
jepickett 0:35c27ebd9e8e 284 {
jepickett 0:35c27ebd9e8e 285 if( _uart.readable())
jepickett 0:35c27ebd9e8e 286 {
jepickett 1:c5327c6d6239 287 // BUGBUG: unsigned char b = _uart.getc();
jepickett 1:c5327c6d6239 288 // Bug with _uart.getc() on FRDM-K64F. Instead read directly from uart
jepickett 0:35c27ebd9e8e 289 char b = UART3->D;
jepickett 0:35c27ebd9e8e 290 replyBuf.push_back( b );
jepickett 0:35c27ebd9e8e 291
jepickett 0:35c27ebd9e8e 292 if( replyBuf.size() == 6)
jepickett 0:35c27ebd9e8e 293 {
jepickett 0:35c27ebd9e8e 294 statusReceived = true;
jepickett 0:35c27ebd9e8e 295 break;
jepickett 0:35c27ebd9e8e 296 }
jepickett 0:35c27ebd9e8e 297 }
jepickett 0:35c27ebd9e8e 298 }
jepickett 0:35c27ebd9e8e 299
jepickett 0:35c27ebd9e8e 300 t.stop();
jepickett 0:35c27ebd9e8e 301 }
jepickett 0:35c27ebd9e8e 302
jepickett 0:35c27ebd9e8e 303 _rxSelect = 0;
jepickett 0:35c27ebd9e8e 304
jepickett 0:35c27ebd9e8e 305 if( statusReceived )
jepickett 0:35c27ebd9e8e 306 {
jepickett 1:c5327c6d6239 307 return replyBuf[4] | statusValid;
jepickett 0:35c27ebd9e8e 308 }
jepickett 0:35c27ebd9e8e 309 else
jepickett 0:35c27ebd9e8e 310 {
jepickett 0:35c27ebd9e8e 311 return 0;
jepickett 0:35c27ebd9e8e 312 }
jepickett 0:35c27ebd9e8e 313
jepickett 0:35c27ebd9e8e 314 }