Telesensorics / Dynamixel

dynamixel protocol engine

Dependents:   RobotArmDemo

Fork of Dynamixel by robot arm demo team

DynamixelBus.cpp@5:8b70bee8059b, 2016-02-04 (annotated)

Committer:
henryrawas
Date:
Thu Feb 04 20:10:09 2016 +0000
Revision:
5:8b70bee8059b
Parent:
4:fe2a6b66cb85 
update copyright

Who changed what in which revision?

UserRevisionLine numberNew contents of line
henryrawas 5:8b70bee8059b 1 /* Copyright (C) 2016 Telesensorics Inc, MIT License
henryrawas 4:fe2a6b66cb85 2 *
henryrawas 4:fe2a6b66cb85 3 * Permission is hereby granted, free of charge, to any person obtaining a copy of this software
henryrawas 4:fe2a6b66cb85 4 * and associated documentation files (the "Software"), to deal in the Software without restriction,
henryrawas 4:fe2a6b66cb85 5 * including without limitation the rights to use, copy, modify, merge, publish, distribute,
henryrawas 4:fe2a6b66cb85 6 * sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is
henryrawas 4:fe2a6b66cb85 7 * furnished to do so, subject to the following conditions:
henryrawas 4:fe2a6b66cb85 8 *
henryrawas 4:fe2a6b66cb85 9 * The above copyright notice and this permission notice shall be included in all copies or
henryrawas 4:fe2a6b66cb85 10 * substantial portions of the Software.
henryrawas 4:fe2a6b66cb85 11 *
henryrawas 4:fe2a6b66cb85 12 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
henryrawas 4:fe2a6b66cb85 13 * BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
henryrawas 4:fe2a6b66cb85 14 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
henryrawas 4:fe2a6b66cb85 15 * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
henryrawas 4:fe2a6b66cb85 16 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
henryrawas 4:fe2a6b66cb85 17 */
henryrawas 4:fe2a6b66cb85 18
jepickett 0:35c27ebd9e8e 19 #include <mbed.h>
jepickett 0:35c27ebd9e8e 20 #include "DynamixelBus.h"
jepickett 0:35c27ebd9e8e 21
jepickett 0:35c27ebd9e8e 22
jepickett 0:35c27ebd9e8e 23 DynamixelBus::DynamixelBus( PinName txUartPin, PinName rxUartPin, PinName txSelectPin, PinName rxSelectPin, int baud )
jepickett 0:35c27ebd9e8e 24 : _uart( txUartPin, rxUartPin), _txSelect(txSelectPin, 0), _rxSelect(rxSelectPin, 0) {
jepickett 1:c5327c6d6239 25 _responseTimeout = .004;
jepickett 0:35c27ebd9e8e 26 _baud = baud;
jepickett 0:35c27ebd9e8e 27 _uart.baud(_baud);
jepickett 0:35c27ebd9e8e 28 _replyDelay = .0012;
jepickett 0:35c27ebd9e8e 29 }
jepickett 0:35c27ebd9e8e 30
jepickett 0:35c27ebd9e8e 31
jepickett 0:35c27ebd9e8e 32 StatusCode DynamixelBus::Ping( ServoId id )
jepickett 0:35c27ebd9e8e 33 {
jepickett 0:35c27ebd9e8e 34 return send_ping( id );
jepickett 0:35c27ebd9e8e 35 }
jepickett 0:35c27ebd9e8e 36
jepickett 0:35c27ebd9e8e 37
jepickett 0:35c27ebd9e8e 38 StatusCode DynamixelBus::Read( ServoId id, unsigned char controlTableStart, unsigned char bytesToRead, CommBuffer& data )
jepickett 0:35c27ebd9e8e 39 {
jepickett 0:35c27ebd9e8e 40 return send_read( id, controlTableStart, bytesToRead, data );
jepickett 0:35c27ebd9e8e 41 }
jepickett 0:35c27ebd9e8e 42
jepickett 0:35c27ebd9e8e 43
jepickett 0:35c27ebd9e8e 44 StatusCode DynamixelBus::Write( ServoId id, unsigned char controlTableStart, const CommBuffer& dataToWrite )
jepickett 0:35c27ebd9e8e 45 {
jepickett 0:35c27ebd9e8e 46 return send_write( id, controlTableStart, dataToWrite );
jepickett 0:35c27ebd9e8e 47 }
jepickett 0:35c27ebd9e8e 48
jepickett 0:35c27ebd9e8e 49
jepickett 0:35c27ebd9e8e 50 StatusCode DynamixelBus::WriteAction( ServoId id, unsigned char start, const CommBuffer& dataToWrite )
jepickett 0:35c27ebd9e8e 51 {
jepickett 0:35c27ebd9e8e 52 return 0;
jepickett 0:35c27ebd9e8e 53 }
jepickett 0:35c27ebd9e8e 54
jepickett 0:35c27ebd9e8e 55
jepickett 0:35c27ebd9e8e 56 void DynamixelBus::TriggerAction( ServoId id )
jepickett 0:35c27ebd9e8e 57 {
jepickett 0:35c27ebd9e8e 58 return;
jepickett 0:35c27ebd9e8e 59 }
jepickett 0:35c27ebd9e8e 60
jepickett 0:35c27ebd9e8e 61
jepickett 0:35c27ebd9e8e 62 StatusCode DynamixelBus::HardReset( ServoId id )
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 StatusCode DynamixelBus::SyncWrite( unsigned char controlTableStart, unsigned char bytesToWrite, vector<SyncIdDataPair>& data )
jepickett 0:35c27ebd9e8e 69 {
jepickett 0:35c27ebd9e8e 70 return 0;
jepickett 0:35c27ebd9e8e 71 }
jepickett 0:35c27ebd9e8e 72
jepickett 0:35c27ebd9e8e 73
jepickett 0:35c27ebd9e8e 74 unsigned char DynamixelBus::CalculateTxPacketChecksum( CommBuffer& packet )
jepickett 0:35c27ebd9e8e 75 {
jepickett 0:35c27ebd9e8e 76 int sum = 0;
jepickett 0:35c27ebd9e8e 77 for( int n = 0; n < packet.size(); n++ )
jepickett 0:35c27ebd9e8e 78 {
jepickett 0:35c27ebd9e8e 79 if( n == 0 || n == 1 )
jepickett 0:35c27ebd9e8e 80 {
jepickett 0:35c27ebd9e8e 81 continue;
jepickett 0:35c27ebd9e8e 82 }
jepickett 0:35c27ebd9e8e 83
jepickett 0:35c27ebd9e8e 84 sum += packet[n];
jepickett 0:35c27ebd9e8e 85 }
jepickett 0:35c27ebd9e8e 86
jepickett 0:35c27ebd9e8e 87 return (~sum & 0xFF); // AX-12 reference manual p. 10, Section 3-2
jepickett 0:35c27ebd9e8e 88 }
jepickett 0:35c27ebd9e8e 89
jepickett 0:35c27ebd9e8e 90
jepickett 0:35c27ebd9e8e 91 // pings a servo at a specified ID
jepickett 0:35c27ebd9e8e 92 //
jepickett 0:35c27ebd9e8e 93 // returns the status code of the unit if found, otherwise returns 0
jepickett 0:35c27ebd9e8e 94 //
jepickett 0:35c27ebd9e8e 95 // REVIEW: will Ping work if AX-12's StatusReturnLevel is set to not return status packets?
jepickett 0:35c27ebd9e8e 96 StatusCode DynamixelBus::send_ping( ServoId id )
jepickett 0:35c27ebd9e8e 97 {
jepickett 0:35c27ebd9e8e 98 // 0xff, 0xff, ID, Length, Instruction(ping), Checksum
jepickett 1:c5327c6d6239 99 CommBuffer txBuf;
jepickett 0:35c27ebd9e8e 100
jepickett 0:35c27ebd9e8e 101 txBuf.push_back( 0xff );
jepickett 0:35c27ebd9e8e 102 txBuf.push_back( 0xff );
jepickett 0:35c27ebd9e8e 103 txBuf.push_back( id );
jepickett 0:35c27ebd9e8e 104 txBuf.push_back( 2 ); // length of remaining packet
jepickett 0:35c27ebd9e8e 105 txBuf.push_back( instructionPing );
jepickett 0:35c27ebd9e8e 106 txBuf.push_back( CalculateTxPacketChecksum( txBuf ) );
jepickett 0:35c27ebd9e8e 107
jepickett 0:35c27ebd9e8e 108 _txSelect = 1;
jepickett 0:35c27ebd9e8e 109
jepickett 0:35c27ebd9e8e 110 for( CommBuffer::iterator itSend = txBuf.begin(); itSend != txBuf.end(); itSend++)
jepickett 0:35c27ebd9e8e 111 {
jepickett 0:35c27ebd9e8e 112 _uart.putc(*itSend);
jepickett 0:35c27ebd9e8e 113 }
jepickett 0:35c27ebd9e8e 114
jepickett 0:35c27ebd9e8e 115 // Wait for data to transmit
jepickett 1:c5327c6d6239 116 double transmitDelay = (double)txBuf.size() * 8.0 * 1.0/(double)_baud;
jepickett 1:c5327c6d6239 117 wait( transmitDelay );
jepickett 0:35c27ebd9e8e 118
jepickett 0:35c27ebd9e8e 119 // swap inputs for half duplex communication
jepickett 0:35c27ebd9e8e 120 _txSelect = 0;
jepickett 0:35c27ebd9e8e 121 _rxSelect = 1;
jepickett 0:35c27ebd9e8e 122
jepickett 1:c5327c6d6239 123 // 0xff, 0xff, ID, Length, Error, Checksum
jepickett 1:c5327c6d6239 124 CommBuffer replyBuf;
jepickett 0:35c27ebd9e8e 125
jepickett 1:c5327c6d6239 126 // we'll only get a reply if it was not broadcast (TODO: and status reply option is selected in servo)
jepickett 0:35c27ebd9e8e 127 bool statusReceived = false;
jepickett 1:c5327c6d6239 128 Timer t;
jepickett 0:35c27ebd9e8e 129 if( id != 0xFE )
jepickett 0:35c27ebd9e8e 130 {
jepickett 1:c5327c6d6239 131 t.start();
jepickett 1:c5327c6d6239 132
jepickett 1:c5327c6d6239 133 while( t.read() < _responseTimeout )
jepickett 0:35c27ebd9e8e 134 {
jepickett 1:c5327c6d6239 135 if( _uart.readable())
jepickett 0:35c27ebd9e8e 136 {
jepickett 1:c5327c6d6239 137 // BUGBUG: unsigned char b = _uart.getc();
jepickett 1:c5327c6d6239 138 // Bug with _uart.getc() on FRDM-K64F. Instead read directly from uart
jepickett 1:c5327c6d6239 139 char b = UART3->D;
jepickett 1:c5327c6d6239 140
jepickett 1:c5327c6d6239 141 replyBuf.push_back( b );
jepickett 0:35c27ebd9e8e 142
jepickett 1:c5327c6d6239 143 if( replyBuf.size() == 6)
jepickett 1:c5327c6d6239 144 {
jepickett 1:c5327c6d6239 145 statusReceived = true;
jepickett 1:c5327c6d6239 146 break;
jepickett 1:c5327c6d6239 147 }
jepickett 0:35c27ebd9e8e 148 }
jepickett 0:35c27ebd9e8e 149 }
jepickett 1:c5327c6d6239 150
jepickett 1:c5327c6d6239 151 t.stop();
jepickett 0:35c27ebd9e8e 152 }
jepickett 1:c5327c6d6239 153
jepickett 0:35c27ebd9e8e 154 _rxSelect = 0;
jepickett 0:35c27ebd9e8e 155
jepickett 0:35c27ebd9e8e 156 if( statusReceived )
jepickett 0:35c27ebd9e8e 157 {
jepickett 1:c5327c6d6239 158 return (unsigned char)(replyBuf[4] | statusValid);
jepickett 0:35c27ebd9e8e 159 }
jepickett 0:35c27ebd9e8e 160 else
jepickett 0:35c27ebd9e8e 161 {
jepickett 0:35c27ebd9e8e 162 return 0;
jepickett 0:35c27ebd9e8e 163 }
jepickett 0:35c27ebd9e8e 164 }
jepickett 0:35c27ebd9e8e 165
jepickett 0:35c27ebd9e8e 166
jepickett 0:35c27ebd9e8e 167 StatusCode DynamixelBus::send_read( ServoId id, unsigned char controlTableStart, unsigned char bytesToRead, CommBuffer& data )
jepickett 0:35c27ebd9e8e 168 {
jepickett 0:35c27ebd9e8e 169 // 0xff, 0xff, ID, Packet Length, Instruction(read), Control Table Start, Length to Read, Checksum
jepickett 0:35c27ebd9e8e 170 CommBuffer txBuf;
jepickett 0:35c27ebd9e8e 171
jepickett 0:35c27ebd9e8e 172 txBuf.push_back( 0xff );
jepickett 0:35c27ebd9e8e 173 txBuf.push_back( 0xff );
jepickett 0:35c27ebd9e8e 174 txBuf.push_back( id );
jepickett 0:35c27ebd9e8e 175 txBuf.push_back( 4 ); // length of remaining packet
jepickett 0:35c27ebd9e8e 176 txBuf.push_back( instructionRead );
jepickett 0:35c27ebd9e8e 177 txBuf.push_back( controlTableStart );
jepickett 0:35c27ebd9e8e 178 txBuf.push_back( bytesToRead );
jepickett 0:35c27ebd9e8e 179 txBuf.push_back( CalculateTxPacketChecksum( txBuf ) );
jepickett 0:35c27ebd9e8e 180
jepickett 1:c5327c6d6239 181 _txSelect = 1;
jepickett 1:c5327c6d6239 182
jepickett 0:35c27ebd9e8e 183 for( CommBuffer::iterator itSend = txBuf.begin(); itSend != txBuf.end(); itSend++)
jepickett 0:35c27ebd9e8e 184 {
jepickett 0:35c27ebd9e8e 185 _uart.putc(*itSend);
jepickett 0:35c27ebd9e8e 186 }
jepickett 0:35c27ebd9e8e 187
jepickett 0:35c27ebd9e8e 188 // Wait for data to transmit
jepickett 1:c5327c6d6239 189 double transmitDelay = (double)txBuf.size() * 8.0 * 1.0/(double)_baud;
jepickett 1:c5327c6d6239 190 wait( transmitDelay );
jepickett 1:c5327c6d6239 191
jepickett 0:35c27ebd9e8e 192 _txSelect = 0;
jepickett 0:35c27ebd9e8e 193 _rxSelect = 1;
jepickett 0:35c27ebd9e8e 194
jepickett 0:35c27ebd9e8e 195 // 0xff, 0xff, ID, Length, Error, Data, Checksum
jepickett 0:35c27ebd9e8e 196 int replySize = 6 + bytesToRead;
jepickett 1:c5327c6d6239 197 CommBuffer replyBuf;
jepickett 0:35c27ebd9e8e 198
jepickett 0:35c27ebd9e8e 199 // we'll only get a reply if it was not broadcast (and status reply option is selected in servo)
jepickett 0:35c27ebd9e8e 200 bool statusReceived = false;
jepickett 1:c5327c6d6239 201 Timer t;
jepickett 0:35c27ebd9e8e 202 if( id != 0xFE )
jepickett 0:35c27ebd9e8e 203 {
jepickett 1:c5327c6d6239 204 t.start();
jepickett 1:c5327c6d6239 205 while( t.read() < _responseTimeout )
jepickett 0:35c27ebd9e8e 206 {
jepickett 0:35c27ebd9e8e 207 if (_uart.readable())
jepickett 0:35c27ebd9e8e 208 {
jepickett 1:c5327c6d6239 209 // BUGBUG: unsigned char b = _uart.getc();
jepickett 1:c5327c6d6239 210 // Bug with _uart.getc() on FRDM-K64F. Instead read directly from uart
jepickett 1:c5327c6d6239 211 char b = UART3->D;
jepickett 1:c5327c6d6239 212 replyBuf.push_back( b );
jepickett 1:c5327c6d6239 213
jepickett 1:c5327c6d6239 214 if( replyBuf.size() == replySize )
jepickett 1:c5327c6d6239 215 {
jepickett 1:c5327c6d6239 216 statusReceived = true;
jepickett 1:c5327c6d6239 217 break;
jepickett 1:c5327c6d6239 218 }
jepickett 0:35c27ebd9e8e 219 }
jepickett 0:35c27ebd9e8e 220 }
jepickett 1:c5327c6d6239 221 t.stop();
jepickett 0:35c27ebd9e8e 222 }
jepickett 0:35c27ebd9e8e 223
jepickett 0:35c27ebd9e8e 224 _rxSelect = 0;
jepickett 0:35c27ebd9e8e 225
jepickett 0:35c27ebd9e8e 226 if( statusReceived )
jepickett 0:35c27ebd9e8e 227 {
jepickett 0:35c27ebd9e8e 228 for( int n = 0; n < bytesToRead; n ++ )
jepickett 0:35c27ebd9e8e 229 {
jepickett 0:35c27ebd9e8e 230 data.push_back( replyBuf[5 + n] );
jepickett 0:35c27ebd9e8e 231 }
jepickett 1:c5327c6d6239 232 return replyBuf[4] | statusValid;
jepickett 0:35c27ebd9e8e 233 }
jepickett 0:35c27ebd9e8e 234 else
jepickett 0:35c27ebd9e8e 235 {
jepickett 0:35c27ebd9e8e 236 return 0;
jepickett 0:35c27ebd9e8e 237 }
jepickett 0:35c27ebd9e8e 238 }
jepickett 0:35c27ebd9e8e 239
jepickett 0:35c27ebd9e8e 240
jepickett 0:35c27ebd9e8e 241 StatusCode DynamixelBus::send_write( ServoId id, unsigned char controlTableStart, const CommBuffer& dataToWrite )
jepickett 0:35c27ebd9e8e 242 {
jepickett 0:35c27ebd9e8e 243 // 0xff, 0xff, ID, Packet Length, Instruction(write), Control Table Start, data to write, Checksum
jepickett 0:35c27ebd9e8e 244 CommBuffer txBuf;
jepickett 0:35c27ebd9e8e 245
jepickett 0:35c27ebd9e8e 246 // build packet
jepickett 0:35c27ebd9e8e 247 txBuf.push_back( 0xff );
jepickett 0:35c27ebd9e8e 248 txBuf.push_back( 0xff );
jepickett 0:35c27ebd9e8e 249 txBuf.push_back( id );
jepickett 0:35c27ebd9e8e 250 txBuf.push_back( 3+dataToWrite.size() ); // length of remaining packet
jepickett 0:35c27ebd9e8e 251 txBuf.push_back( instructionWrite );
jepickett 0:35c27ebd9e8e 252 txBuf.push_back( controlTableStart );
jepickett 0:35c27ebd9e8e 253 for( CommBuffer::const_iterator itD = dataToWrite.begin(); itD != dataToWrite.end(); itD++)
jepickett 0:35c27ebd9e8e 254 {
jepickett 0:35c27ebd9e8e 255 txBuf.push_back(*itD);
jepickett 0:35c27ebd9e8e 256 }
jepickett 0:35c27ebd9e8e 257 txBuf.push_back( CalculateTxPacketChecksum( txBuf ) );
jepickett 0:35c27ebd9e8e 258
jepickett 0:35c27ebd9e8e 259 // enable transmission
jepickett 0:35c27ebd9e8e 260 _txSelect = 1;
jepickett 0:35c27ebd9e8e 261
jepickett 0:35c27ebd9e8e 262 for( CommBuffer::iterator itSend = txBuf.begin(); itSend != txBuf.end(); itSend++)
jepickett 0:35c27ebd9e8e 263 {
jepickett 0:35c27ebd9e8e 264 _uart.putc(*itSend);
jepickett 0:35c27ebd9e8e 265 }
jepickett 0:35c27ebd9e8e 266
jepickett 0:35c27ebd9e8e 267 // Wait for data to transmit
jepickett 0:35c27ebd9e8e 268 double transmitDelay = (double)txBuf.size() * 8.0 * 1.0/(double)_baud;
jepickett 0:35c27ebd9e8e 269 wait( transmitDelay );
jepickett 0:35c27ebd9e8e 270
jepickett 0:35c27ebd9e8e 271 // switch inputs for half duplex operation
jepickett 0:35c27ebd9e8e 272 _txSelect = 0;
jepickett 0:35c27ebd9e8e 273 _rxSelect = 1;
jepickett 0:35c27ebd9e8e 274
jepickett 0:35c27ebd9e8e 275 // 0xff, 0xff, ID, Length, Error, Data, Checksum
jepickett 0:35c27ebd9e8e 276 CommBuffer replyBuf;
jepickett 0:35c27ebd9e8e 277
jepickett 0:35c27ebd9e8e 278 // we'll only get a reply if it was not broadcast (TODO: and status reply option is selected in servo)
jepickett 0:35c27ebd9e8e 279 bool statusReceived = false;
jepickett 0:35c27ebd9e8e 280 Timer t;
jepickett 0:35c27ebd9e8e 281 if( id != 0xFE )
jepickett 0:35c27ebd9e8e 282 {
jepickett 0:35c27ebd9e8e 283 t.start();
jepickett 0:35c27ebd9e8e 284
jepickett 1:c5327c6d6239 285 while( t.read() < _responseTimeout )
jepickett 0:35c27ebd9e8e 286 {
jepickett 0:35c27ebd9e8e 287 if( _uart.readable())
jepickett 0:35c27ebd9e8e 288 {
jepickett 1:c5327c6d6239 289 // BUGBUG: unsigned char b = _uart.getc();
jepickett 1:c5327c6d6239 290 // Bug with _uart.getc() on FRDM-K64F. Instead read directly from uart
jepickett 0:35c27ebd9e8e 291 char b = UART3->D;
jepickett 0:35c27ebd9e8e 292 replyBuf.push_back( b );
jepickett 0:35c27ebd9e8e 293
jepickett 0:35c27ebd9e8e 294 if( replyBuf.size() == 6)
jepickett 0:35c27ebd9e8e 295 {
jepickett 0:35c27ebd9e8e 296 statusReceived = true;
jepickett 0:35c27ebd9e8e 297 break;
jepickett 0:35c27ebd9e8e 298 }
jepickett 0:35c27ebd9e8e 299 }
jepickett 0:35c27ebd9e8e 300 }
jepickett 0:35c27ebd9e8e 301
jepickett 0:35c27ebd9e8e 302 t.stop();
jepickett 0:35c27ebd9e8e 303 }
jepickett 0:35c27ebd9e8e 304
jepickett 0:35c27ebd9e8e 305 _rxSelect = 0;
jepickett 0:35c27ebd9e8e 306
jepickett 0:35c27ebd9e8e 307 if( statusReceived )
jepickett 0:35c27ebd9e8e 308 {
jepickett 1:c5327c6d6239 309 return replyBuf[4] | statusValid;
jepickett 0:35c27ebd9e8e 310 }
jepickett 0:35c27ebd9e8e 311 else
jepickett 0:35c27ebd9e8e 312 {
jepickett 0:35c27ebd9e8e 313 return 0;
jepickett 0:35c27ebd9e8e 314 }
jepickett 0:35c27ebd9e8e 315
jepickett 0:35c27ebd9e8e 316 }