BE@R lab / MX28

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:   MX_control

Fork of MX28 by Georgios Petrou

MX28.cpp@3:7929abcdc22f, 2014-10-24 (annotated)

Committer:
soulx
Date:
Fri Oct 24 08:57:38 2014 +0000
Revision:
3:7929abcdc22f
Parent:
1:5f537df9dca8 
change serialhalfduplex to iSerial because ithas ring buffer

Who changed what in which revision?

UserRevisionLine numberNew contents of line
GIPetrou 1:5f537df9dca8 1 /* Copyright (c) 2012 Georgios Petrou, MIT License
GIPetrou 1:5f537df9dca8 2 *
GIPetrou 1:5f537df9dca8 3 * Permission is hereby granted, free of charge, to any person obtaining a copy of this software
GIPetrou 1:5f537df9dca8 4 * and associated documentation files (the "Software"), to deal in the Software without restriction,
GIPetrou 1:5f537df9dca8 5 * including without limitation the rights to use, copy, modify, merge, publish, distribute,
GIPetrou 1:5f537df9dca8 6 * sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is
GIPetrou 1:5f537df9dca8 7 * furnished to do so, subject to the following conditions:
GIPetrou 1:5f537df9dca8 8 *
GIPetrou 1:5f537df9dca8 9 * The above copyright notice and this permission notice shall be included in all copies or
GIPetrou 1:5f537df9dca8 10 * substantial portions of the Software.
GIPetrou 1:5f537df9dca8 11 *
GIPetrou 1:5f537df9dca8 12 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
GIPetrou 1:5f537df9dca8 13 * BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
GIPetrou 1:5f537df9dca8 14 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
GIPetrou 1:5f537df9dca8 15 * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
GIPetrou 1:5f537df9dca8 16 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
GIPetrou 1:5f537df9dca8 17 */
GIPetrou 1:5f537df9dca8 18
GIPetrou 1:5f537df9dca8 19 #include "MX28.h"
soulx 3:7929abcdc22f 20 #include "mbed.h"
soulx 3:7929abcdc22f 21
soulx 3:7929abcdc22f 22 #define DELAY_DIR 90 // unit in us
soulx 3:7929abcdc22f 23
soulx 3:7929abcdc22f 24 DigitalOut dirc_rs485(PIN_RS485_DIRC);
soulx 3:7929abcdc22f 25
GIPetrou 1:5f537df9dca8 26
GIPetrou 1:5f537df9dca8 27 uint8_t MX28::CommunicatePacket(MX28_PROTOCOL_PACKET *packet)
GIPetrou 1:5f537df9dca8 28 {
GIPetrou 1:5f537df9dca8 29 uint8_t currentParameter = 0;
GIPetrou 1:5f537df9dca8 30 bool isWholePacket = false;
GIPetrou 1:5f537df9dca8 31 uint8_t encoderState = WAIT_ON_HEADER_0;
GIPetrou 1:5f537df9dca8 32
GIPetrou 1:5f537df9dca8 33 packet->checkSum = Utilities::GetCheckSum((uint8_t*)&(packet->servoId), packet->length + 1);
GIPetrou 1:5f537df9dca8 34
GIPetrou 1:5f537df9dca8 35 Timer timer;
GIPetrou 1:5f537df9dca8 36 timer.start();
GIPetrou 1:5f537df9dca8 37
GIPetrou 1:5f537df9dca8 38 while((timer.read_ms() < MX28_PROTOCOL_COMMAND_RESPONSE_TIMEOUT_MS) && (!isWholePacket))
GIPetrou 1:5f537df9dca8 39 {
GIPetrou 1:5f537df9dca8 40 if(servoSerialHalfDuplex->writeable())
GIPetrou 1:5f537df9dca8 41 {
soulx 3:7929abcdc22f 42
GIPetrou 1:5f537df9dca8 43 switch(encoderState)
GIPetrou 1:5f537df9dca8 44 {
GIPetrou 1:5f537df9dca8 45 case WAIT_ON_HEADER_0:
GIPetrou 1:5f537df9dca8 46 {
GIPetrou 1:5f537df9dca8 47 #ifdef MX28_DEBUG
GIPetrou 1:5f537df9dca8 48 pc->printf("Write: 0x%02X ", MX28_PROTOCOL_HEADER_0);
GIPetrou 1:5f537df9dca8 49 #endif
soulx 3:7929abcdc22f 50 dirc_rs485=1;
soulx 3:7929abcdc22f 51 // wait_us(DELAY_DIR);
soulx 3:7929abcdc22f 52
GIPetrou 1:5f537df9dca8 53 servoSerialHalfDuplex->putc(MX28_PROTOCOL_HEADER_0);
soulx 3:7929abcdc22f 54 // dirc_rs485=0;
soulx 3:7929abcdc22f 55 // wait_us(DELAY_DIR);
soulx 3:7929abcdc22f 56
GIPetrou 1:5f537df9dca8 57
GIPetrou 1:5f537df9dca8 58 encoderState = WAIT_ON_HEADER_1;
GIPetrou 1:5f537df9dca8 59
GIPetrou 1:5f537df9dca8 60 break;
GIPetrou 1:5f537df9dca8 61 }
GIPetrou 1:5f537df9dca8 62 case WAIT_ON_HEADER_1:
GIPetrou 1:5f537df9dca8 63 {
GIPetrou 1:5f537df9dca8 64 #ifdef MX28_DEBUG
GIPetrou 1:5f537df9dca8 65 pc->printf("0x%02X ", MX28_PROTOCOL_HEADER_1);
GIPetrou 1:5f537df9dca8 66 #endif
soulx 3:7929abcdc22f 67
soulx 3:7929abcdc22f 68 dirc_rs485=1;
soulx 3:7929abcdc22f 69 // wait_us(DELAY_DIR);
soulx 3:7929abcdc22f 70
GIPetrou 1:5f537df9dca8 71 servoSerialHalfDuplex->putc(MX28_PROTOCOL_HEADER_1);
soulx 3:7929abcdc22f 72 // dirc_rs485=0;
soulx 3:7929abcdc22f 73 // wait_us(DELAY_DIR);
GIPetrou 1:5f537df9dca8 74
GIPetrou 1:5f537df9dca8 75 encoderState = WAIT_ON_SERVO_ID;
GIPetrou 1:5f537df9dca8 76
GIPetrou 1:5f537df9dca8 77 break;
GIPetrou 1:5f537df9dca8 78 }
GIPetrou 1:5f537df9dca8 79 case WAIT_ON_SERVO_ID:
GIPetrou 1:5f537df9dca8 80 {
GIPetrou 1:5f537df9dca8 81 #ifdef MX28_DEBUG
GIPetrou 1:5f537df9dca8 82 pc->printf("0x%02X ", packet->servoId);
GIPetrou 1:5f537df9dca8 83 #endif
soulx 3:7929abcdc22f 84 dirc_rs485=1;
soulx 3:7929abcdc22f 85 // wait_us(DELAY_DIR);
GIPetrou 1:5f537df9dca8 86
GIPetrou 1:5f537df9dca8 87 servoSerialHalfDuplex->putc(packet->servoId);
soulx 3:7929abcdc22f 88 // dirc_rs485=0;
soulx 3:7929abcdc22f 89 // wait_us(DELAY_DIR);
GIPetrou 1:5f537df9dca8 90
GIPetrou 1:5f537df9dca8 91 encoderState = WAIT_ON_LENGTH;
GIPetrou 1:5f537df9dca8 92
GIPetrou 1:5f537df9dca8 93 break;
GIPetrou 1:5f537df9dca8 94 }
GIPetrou 1:5f537df9dca8 95 case WAIT_ON_LENGTH:
GIPetrou 1:5f537df9dca8 96 {
GIPetrou 1:5f537df9dca8 97 #ifdef MX28_DEBUG
GIPetrou 1:5f537df9dca8 98 pc->printf("0x%02X ", packet->length);
GIPetrou 1:5f537df9dca8 99 #endif
soulx 3:7929abcdc22f 100 dirc_rs485=1;
soulx 3:7929abcdc22f 101 // wait_us(DELAY_DIR);
GIPetrou 1:5f537df9dca8 102 servoSerialHalfDuplex->putc(packet->length);
soulx 3:7929abcdc22f 103 // dirc_rs485=0;
GIPetrou 1:5f537df9dca8 104
GIPetrou 1:5f537df9dca8 105 encoderState = WAIT_ON_INSTRUCTION_ERROR_ID;
GIPetrou 1:5f537df9dca8 106
GIPetrou 1:5f537df9dca8 107 break;
GIPetrou 1:5f537df9dca8 108 }
GIPetrou 1:5f537df9dca8 109 case WAIT_ON_INSTRUCTION_ERROR_ID:
GIPetrou 1:5f537df9dca8 110 {
GIPetrou 1:5f537df9dca8 111 #ifdef MX28_DEBUG
GIPetrou 1:5f537df9dca8 112 pc->printf("0x%02X ", packet->instructionErrorId);
GIPetrou 1:5f537df9dca8 113 #endif
soulx 3:7929abcdc22f 114 dirc_rs485=1;
soulx 3:7929abcdc22f 115 // wait_us(DELAY_DIR);
GIPetrou 1:5f537df9dca8 116
GIPetrou 1:5f537df9dca8 117 servoSerialHalfDuplex->putc(packet->instructionErrorId);
GIPetrou 1:5f537df9dca8 118
GIPetrou 1:5f537df9dca8 119 if(packet->length > 2)
GIPetrou 1:5f537df9dca8 120 encoderState = WAIT_ON_PARAMETER;
GIPetrou 1:5f537df9dca8 121 else
GIPetrou 1:5f537df9dca8 122 encoderState = WAIT_ON_CHECK_SUM;
GIPetrou 1:5f537df9dca8 123
GIPetrou 1:5f537df9dca8 124 break;
GIPetrou 1:5f537df9dca8 125 }
GIPetrou 1:5f537df9dca8 126 case WAIT_ON_PARAMETER:
GIPetrou 1:5f537df9dca8 127 {
GIPetrou 1:5f537df9dca8 128 #ifdef MX28_DEBUG
GIPetrou 1:5f537df9dca8 129 pc->printf("0x%02X ", packet->parameter[currentParameter]);
GIPetrou 1:5f537df9dca8 130 #endif
soulx 3:7929abcdc22f 131 dirc_rs485=1;
soulx 3:7929abcdc22f 132 // wait_us(DELAY_DIR);
soulx 3:7929abcdc22f 133
GIPetrou 1:5f537df9dca8 134 servoSerialHalfDuplex->putc(packet->parameter[currentParameter]);
GIPetrou 1:5f537df9dca8 135
GIPetrou 1:5f537df9dca8 136 if(++currentParameter == packet->length - 2)
GIPetrou 1:5f537df9dca8 137 encoderState = WAIT_ON_CHECK_SUM;
GIPetrou 1:5f537df9dca8 138
GIPetrou 1:5f537df9dca8 139 break;
GIPetrou 1:5f537df9dca8 140 }
GIPetrou 1:5f537df9dca8 141 case WAIT_ON_CHECK_SUM:
GIPetrou 1:5f537df9dca8 142 {
GIPetrou 1:5f537df9dca8 143 #ifdef MX28_DEBUG
GIPetrou 1:5f537df9dca8 144 pc->printf("0x%02X\r\n", packet->checkSum);
GIPetrou 1:5f537df9dca8 145 #endif
soulx 3:7929abcdc22f 146 dirc_rs485=1;
soulx 3:7929abcdc22f 147 // wait_us(DELAY_DIR);
GIPetrou 1:5f537df9dca8 148
GIPetrou 1:5f537df9dca8 149 servoSerialHalfDuplex->putc(packet->checkSum);
soulx 3:7929abcdc22f 150
soulx 3:7929abcdc22f 151
GIPetrou 1:5f537df9dca8 152 encoderState = WAIT_ON_HEADER_0;
GIPetrou 1:5f537df9dca8 153 isWholePacket = true;
GIPetrou 1:5f537df9dca8 154
GIPetrou 1:5f537df9dca8 155 break;
GIPetrou 1:5f537df9dca8 156 }
GIPetrou 1:5f537df9dca8 157 }
GIPetrou 1:5f537df9dca8 158 }
GIPetrou 1:5f537df9dca8 159 }
GIPetrou 1:5f537df9dca8 160
soulx 3:7929abcdc22f 161 wait_us(DELAY_DIR);
soulx 3:7929abcdc22f 162 dirc_rs485=0;
soulx 3:7929abcdc22f 163
GIPetrou 1:5f537df9dca8 164 #ifdef MX28_DEBUG
soulx 3:7929abcdc22f 165 pc->printf("Timer: %d ms\r\n", timer.read_ms());
GIPetrou 1:5f537df9dca8 166 #endif
GIPetrou 1:5f537df9dca8 167
GIPetrou 1:5f537df9dca8 168 timer.stop();
GIPetrou 1:5f537df9dca8 169
soulx 3:7929abcdc22f 170
soulx 3:7929abcdc22f 171
GIPetrou 1:5f537df9dca8 172 if(!isWholePacket)
GIPetrou 1:5f537df9dca8 173 {
GIPetrou 1:5f537df9dca8 174 #ifdef MX28_DEBUG
GIPetrou 1:5f537df9dca8 175 pc->printf("Error: Write response timeout.\r\n");
GIPetrou 1:5f537df9dca8 176 #endif
GIPetrou 1:5f537df9dca8 177
GIPetrou 1:5f537df9dca8 178 return MX28_ERRBIT_WRITE_TIMEOUT;
GIPetrou 1:5f537df9dca8 179 }
GIPetrou 1:5f537df9dca8 180
GIPetrou 1:5f537df9dca8 181 if( packet->servoId == MX28_PROTOCOL_BROADCAST_ID ||
GIPetrou 1:5f537df9dca8 182 packet->instructionErrorId == MX28_ACTION ||
GIPetrou 1:5f537df9dca8 183 packet->instructionErrorId == MX28_SYNC_WRITE)
GIPetrou 1:5f537df9dca8 184
GIPetrou 1:5f537df9dca8 185 return MX28_ERRBIT_NONE;
GIPetrou 1:5f537df9dca8 186
GIPetrou 1:5f537df9dca8 187 currentParameter = 0;
GIPetrou 1:5f537df9dca8 188 isWholePacket = false;
GIPetrou 1:5f537df9dca8 189 uint8_t decoderState = WAIT_ON_HEADER_0;
soulx 3:7929abcdc22f 190
GIPetrou 1:5f537df9dca8 191 timer.reset();
GIPetrou 1:5f537df9dca8 192 timer.start();
GIPetrou 1:5f537df9dca8 193
GIPetrou 1:5f537df9dca8 194 while((timer.read_ms() < MX28_PROTOCOL_COMMAND_RESPONSE_TIMEOUT_MS) && (!isWholePacket))
GIPetrou 1:5f537df9dca8 195 {
GIPetrou 1:5f537df9dca8 196 if(servoSerialHalfDuplex->readable())
GIPetrou 1:5f537df9dca8 197 {
GIPetrou 1:5f537df9dca8 198 switch(decoderState)
GIPetrou 1:5f537df9dca8 199 {
GIPetrou 1:5f537df9dca8 200 case WAIT_ON_HEADER_0:
GIPetrou 1:5f537df9dca8 201 {
soulx 3:7929abcdc22f 202 dirc_rs485=0;
soulx 3:7929abcdc22f 203 // wait_us(DELAY_DIR);
GIPetrou 1:5f537df9dca8 204 uint8_t mx28ProtocolHeader0 = servoSerialHalfDuplex->getc();
GIPetrou 1:5f537df9dca8 205
GIPetrou 1:5f537df9dca8 206 #ifdef MX28_DEBUG
GIPetrou 1:5f537df9dca8 207 pc->printf("Read: 0x%02X ", mx28ProtocolHeader0);
GIPetrou 1:5f537df9dca8 208 #endif
GIPetrou 1:5f537df9dca8 209
GIPetrou 1:5f537df9dca8 210 decoderState = WAIT_ON_HEADER_1;
GIPetrou 1:5f537df9dca8 211
GIPetrou 1:5f537df9dca8 212 break;
GIPetrou 1:5f537df9dca8 213 }
GIPetrou 1:5f537df9dca8 214 case WAIT_ON_HEADER_1:
GIPetrou 1:5f537df9dca8 215 {
soulx 3:7929abcdc22f 216 dirc_rs485=0;
soulx 3:7929abcdc22f 217 //wait_us(DELAY_DIR);
GIPetrou 1:5f537df9dca8 218 uint8_t mx28ProtocolHeader1 = servoSerialHalfDuplex->getc();
GIPetrou 1:5f537df9dca8 219
GIPetrou 1:5f537df9dca8 220 #ifdef MX28_DEBUG
GIPetrou 1:5f537df9dca8 221 pc->printf("0x%02X ", mx28ProtocolHeader1);
GIPetrou 1:5f537df9dca8 222 #endif
GIPetrou 1:5f537df9dca8 223
GIPetrou 1:5f537df9dca8 224 decoderState = WAIT_ON_SERVO_ID;
GIPetrou 1:5f537df9dca8 225
GIPetrou 1:5f537df9dca8 226 break;
GIPetrou 1:5f537df9dca8 227 }
GIPetrou 1:5f537df9dca8 228 case WAIT_ON_SERVO_ID:
GIPetrou 1:5f537df9dca8 229 {
soulx 3:7929abcdc22f 230 dirc_rs485=0;
soulx 3:7929abcdc22f 231 //wait_us(DELAY_DIR);
GIPetrou 1:5f537df9dca8 232 packet->servoId = servoSerialHalfDuplex->getc();
GIPetrou 1:5f537df9dca8 233
GIPetrou 1:5f537df9dca8 234 #ifdef MX28_DEBUG
GIPetrou 1:5f537df9dca8 235 pc->printf("0x%02X ", packet->servoId);
GIPetrou 1:5f537df9dca8 236 #endif
GIPetrou 1:5f537df9dca8 237
GIPetrou 1:5f537df9dca8 238 decoderState = WAIT_ON_LENGTH;
GIPetrou 1:5f537df9dca8 239
GIPetrou 1:5f537df9dca8 240 break;
GIPetrou 1:5f537df9dca8 241 }
GIPetrou 1:5f537df9dca8 242 case WAIT_ON_LENGTH:
GIPetrou 1:5f537df9dca8 243 {
soulx 3:7929abcdc22f 244 dirc_rs485=0;
soulx 3:7929abcdc22f 245 // wait_us(DELAY_DIR);
GIPetrou 1:5f537df9dca8 246 packet->length = servoSerialHalfDuplex->getc();
GIPetrou 1:5f537df9dca8 247
GIPetrou 1:5f537df9dca8 248 #ifdef MX28_DEBUG
GIPetrou 1:5f537df9dca8 249 pc->printf("0x%02X ", packet->length);
GIPetrou 1:5f537df9dca8 250 #endif
GIPetrou 1:5f537df9dca8 251
GIPetrou 1:5f537df9dca8 252 decoderState = WAIT_ON_INSTRUCTION_ERROR_ID;
GIPetrou 1:5f537df9dca8 253
GIPetrou 1:5f537df9dca8 254 break;
GIPetrou 1:5f537df9dca8 255 }
GIPetrou 1:5f537df9dca8 256 case WAIT_ON_INSTRUCTION_ERROR_ID:
GIPetrou 1:5f537df9dca8 257 {
soulx 3:7929abcdc22f 258 dirc_rs485=0;
soulx 3:7929abcdc22f 259 // wait_us(DELAY_DIR);
GIPetrou 1:5f537df9dca8 260 packet->instructionErrorId = servoSerialHalfDuplex->getc();
GIPetrou 1:5f537df9dca8 261
GIPetrou 1:5f537df9dca8 262 #ifdef MX28_DEBUG
GIPetrou 1:5f537df9dca8 263 pc->printf("0x%02X ", packet->instructionErrorId);
GIPetrou 1:5f537df9dca8 264 #endif
GIPetrou 1:5f537df9dca8 265
GIPetrou 1:5f537df9dca8 266 if(packet->length > 2)
GIPetrou 1:5f537df9dca8 267 decoderState = WAIT_ON_PARAMETER;
GIPetrou 1:5f537df9dca8 268 else
GIPetrou 1:5f537df9dca8 269 decoderState = WAIT_ON_CHECK_SUM;
GIPetrou 1:5f537df9dca8 270
GIPetrou 1:5f537df9dca8 271 break;
GIPetrou 1:5f537df9dca8 272 }
GIPetrou 1:5f537df9dca8 273 case WAIT_ON_PARAMETER:
GIPetrou 1:5f537df9dca8 274 {
soulx 3:7929abcdc22f 275 dirc_rs485=0;
soulx 3:7929abcdc22f 276 // wait_us(DELAY_DIR);
GIPetrou 1:5f537df9dca8 277 uint8_t parameter = servoSerialHalfDuplex->getc();
GIPetrou 1:5f537df9dca8 278 packet->parameter[currentParameter] = parameter;
GIPetrou 1:5f537df9dca8 279
GIPetrou 1:5f537df9dca8 280 #ifdef MX28_DEBUG
GIPetrou 1:5f537df9dca8 281 pc->printf("0x%02X ", parameter);
GIPetrou 1:5f537df9dca8 282 #endif
GIPetrou 1:5f537df9dca8 283
GIPetrou 1:5f537df9dca8 284 if(++currentParameter == packet->length - 2)
GIPetrou 1:5f537df9dca8 285 decoderState = WAIT_ON_CHECK_SUM;
GIPetrou 1:5f537df9dca8 286
GIPetrou 1:5f537df9dca8 287 break;
GIPetrou 1:5f537df9dca8 288 }
GIPetrou 1:5f537df9dca8 289 case WAIT_ON_CHECK_SUM:
GIPetrou 1:5f537df9dca8 290 {
soulx 3:7929abcdc22f 291 dirc_rs485=0;
soulx 3:7929abcdc22f 292 // wait_us(DELAY_DIR);
GIPetrou 1:5f537df9dca8 293 packet->checkSum = servoSerialHalfDuplex->getc();
GIPetrou 1:5f537df9dca8 294
GIPetrou 1:5f537df9dca8 295 #ifdef MX28_DEBUG
soulx 3:7929abcdc22f 296 pc->printf("sum =0x%02X\r\n", packet->checkSum);
GIPetrou 1:5f537df9dca8 297 #endif
GIPetrou 1:5f537df9dca8 298
GIPetrou 1:5f537df9dca8 299 decoderState = WAIT_ON_HEADER_0;
GIPetrou 1:5f537df9dca8 300 isWholePacket = true;
GIPetrou 1:5f537df9dca8 301
GIPetrou 1:5f537df9dca8 302 break;
GIPetrou 1:5f537df9dca8 303 }
GIPetrou 1:5f537df9dca8 304 }
GIPetrou 1:5f537df9dca8 305 }
GIPetrou 1:5f537df9dca8 306 }
GIPetrou 1:5f537df9dca8 307
GIPetrou 1:5f537df9dca8 308 #ifdef MX28_DEBUG
GIPetrou 1:5f537df9dca8 309 pc->printf("Timer: %d ms\r\n", timer.read_ms());
GIPetrou 1:5f537df9dca8 310 #endif
GIPetrou 1:5f537df9dca8 311
GIPetrou 1:5f537df9dca8 312 timer.stop();
GIPetrou 1:5f537df9dca8 313
GIPetrou 1:5f537df9dca8 314 if(!isWholePacket)
GIPetrou 1:5f537df9dca8 315 {
GIPetrou 1:5f537df9dca8 316 #ifdef MX28_DEBUG
GIPetrou 1:5f537df9dca8 317 pc->printf("Error: Read response timeout\r\n");
GIPetrou 1:5f537df9dca8 318 #endif
GIPetrou 1:5f537df9dca8 319
GIPetrou 1:5f537df9dca8 320 return MX28_ERRBIT_READ_TIMEOUT;
GIPetrou 1:5f537df9dca8 321 }
GIPetrou 1:5f537df9dca8 322
GIPetrou 1:5f537df9dca8 323 if(packet->instructionErrorId != MX28_ERRBIT_NONE)
GIPetrou 1:5f537df9dca8 324 {
GIPetrou 1:5f537df9dca8 325 #ifdef MX28_DEBUG
GIPetrou 1:5f537df9dca8 326 if(packet->instructionErrorId & MX28_ERRBIT_VOLTAGE)
GIPetrou 1:5f537df9dca8 327 pc->printf("Error: Input voltage error\r\n");
GIPetrou 1:5f537df9dca8 328 if(packet->instructionErrorId & MX28_ERRBIT_ANGLE)
GIPetrou 1:5f537df9dca8 329 pc->printf("Error: Angle limit error\r\n");
GIPetrou 1:5f537df9dca8 330 if(packet->instructionErrorId & MX28_ERRBIT_OVERHEAT)
GIPetrou 1:5f537df9dca8 331 pc->printf("Error: Overheat error\r\n");
GIPetrou 1:5f537df9dca8 332 if(packet->instructionErrorId & MX28_ERRBIT_RANGE)
GIPetrou 1:5f537df9dca8 333 pc->printf("Error: Out of range error\r\n");
GIPetrou 1:5f537df9dca8 334 if(packet->instructionErrorId & MX28_ERRBIT_CHECKSUM)
GIPetrou 1:5f537df9dca8 335 pc->printf("Error: Checksum error\r\n");
GIPetrou 1:5f537df9dca8 336 if(packet->instructionErrorId & MX28_ERRBIT_OVERLOAD)
GIPetrou 1:5f537df9dca8 337 pc->printf("Error: Overload error\r\n");
GIPetrou 1:5f537df9dca8 338 if(packet->instructionErrorId & MX28_ERRBIT_INSTRUCTION)
GIPetrou 1:5f537df9dca8 339 pc->printf("Error: Instruction code error\r\n");
GIPetrou 1:5f537df9dca8 340 #endif
GIPetrou 1:5f537df9dca8 341
GIPetrou 1:5f537df9dca8 342 return packet->instructionErrorId;
GIPetrou 1:5f537df9dca8 343 }
GIPetrou 1:5f537df9dca8 344
GIPetrou 1:5f537df9dca8 345 if(packet->checkSum != Utilities::GetCheckSum((uint8_t*)&(packet->servoId), packet->length + 1))
GIPetrou 1:5f537df9dca8 346 {
GIPetrou 1:5f537df9dca8 347 #ifdef MX28_DEBUG
GIPetrou 1:5f537df9dca8 348 pc->printf("Error: Master got wrong checksum\r\n");
GIPetrou 1:5f537df9dca8 349 #endif
GIPetrou 1:5f537df9dca8 350
GIPetrou 1:5f537df9dca8 351 return MX28_ERRBIT_MASTER_CHECKSUM;
GIPetrou 1:5f537df9dca8 352 }
GIPetrou 1:5f537df9dca8 353
GIPetrou 1:5f537df9dca8 354 return MX28_ERRBIT_NONE;
GIPetrou 1:5f537df9dca8 355 }
GIPetrou 1:5f537df9dca8 356
GIPetrou 1:5f537df9dca8 357 uint8_t MX28::GetModelNumber(uint8_t servoId, uint16_t *modelNumber)
GIPetrou 1:5f537df9dca8 358 {
GIPetrou 1:5f537df9dca8 359 MX28_PROTOCOL_PACKET packet;
GIPetrou 1:5f537df9dca8 360
GIPetrou 1:5f537df9dca8 361 packet.servoId = servoId;
GIPetrou 1:5f537df9dca8 362 packet.length = 4;
GIPetrou 1:5f537df9dca8 363 packet.instructionErrorId = MX28_READ_DATA;
GIPetrou 1:5f537df9dca8 364 packet.parameter[0] = MX28_MODEL_NUMBER_L;
GIPetrou 1:5f537df9dca8 365 packet.parameter[1] = 0x02;
GIPetrou 1:5f537df9dca8 366
GIPetrou 1:5f537df9dca8 367 uint8_t status = CommunicatePacket(&packet);
GIPetrou 1:5f537df9dca8 368
GIPetrou 1:5f537df9dca8 369 if(status == MX28_ERRBIT_NONE)
GIPetrou 1:5f537df9dca8 370 {
GIPetrou 1:5f537df9dca8 371 *modelNumber = Utilities::ConvertUInt8ArrayToUInt16(packet.parameter);
GIPetrou 1:5f537df9dca8 372
GIPetrou 1:5f537df9dca8 373 #ifdef MX28_DEBUG
GIPetrou 1:5f537df9dca8 374 pc->printf("Get model number: %hu\r\n", *modelNumber);
GIPetrou 1:5f537df9dca8 375 #endif
GIPetrou 1:5f537df9dca8 376 }
GIPetrou 1:5f537df9dca8 377
GIPetrou 1:5f537df9dca8 378 return status;
GIPetrou 1:5f537df9dca8 379 }
GIPetrou 1:5f537df9dca8 380
GIPetrou 1:5f537df9dca8 381 uint8_t MX28::GetFirmwareVersion(uint8_t servoId, uint8_t *firmwareVersion)
GIPetrou 1:5f537df9dca8 382 {
GIPetrou 1:5f537df9dca8 383 MX28_PROTOCOL_PACKET packet;
GIPetrou 1:5f537df9dca8 384
GIPetrou 1:5f537df9dca8 385 packet.servoId = servoId;
GIPetrou 1:5f537df9dca8 386 packet.length = 4;
GIPetrou 1:5f537df9dca8 387 packet.instructionErrorId = MX28_READ_DATA;
GIPetrou 1:5f537df9dca8 388 packet.parameter[0] = MX28_VERSION;
GIPetrou 1:5f537df9dca8 389 packet.parameter[1] = 0x01;
GIPetrou 1:5f537df9dca8 390
GIPetrou 1:5f537df9dca8 391 uint8_t status = CommunicatePacket(&packet);
GIPetrou 1:5f537df9dca8 392
GIPetrou 1:5f537df9dca8 393 if(status == MX28_ERRBIT_NONE)
GIPetrou 1:5f537df9dca8 394 {
GIPetrou 1:5f537df9dca8 395 *firmwareVersion = packet.parameter[0];
GIPetrou 1:5f537df9dca8 396
GIPetrou 1:5f537df9dca8 397 #ifdef MX28_DEBUG
GIPetrou 1:5f537df9dca8 398 pc->printf("Get firmware version: 0x%02X\r\n", *firmwareVersion);
GIPetrou 1:5f537df9dca8 399 #endif
GIPetrou 1:5f537df9dca8 400 }
GIPetrou 1:5f537df9dca8 401
GIPetrou 1:5f537df9dca8 402 return status;
GIPetrou 1:5f537df9dca8 403 }
GIPetrou 1:5f537df9dca8 404
GIPetrou 1:5f537df9dca8 405 uint8_t MX28::GetId(uint8_t servoId, uint8_t *id)
GIPetrou 1:5f537df9dca8 406 {
GIPetrou 1:5f537df9dca8 407 MX28_PROTOCOL_PACKET packet;
GIPetrou 1:5f537df9dca8 408
GIPetrou 1:5f537df9dca8 409 packet.servoId = servoId;
GIPetrou 1:5f537df9dca8 410 packet.length = 4;
GIPetrou 1:5f537df9dca8 411 packet.instructionErrorId = MX28_READ_DATA;
GIPetrou 1:5f537df9dca8 412 packet.parameter[0] = MX28_ID;
GIPetrou 1:5f537df9dca8 413 packet.parameter[1] = 0x01;
GIPetrou 1:5f537df9dca8 414
GIPetrou 1:5f537df9dca8 415 uint8_t status = CommunicatePacket(&packet);
GIPetrou 1:5f537df9dca8 416
GIPetrou 1:5f537df9dca8 417 if(status == MX28_ERRBIT_NONE)
GIPetrou 1:5f537df9dca8 418 {
GIPetrou 1:5f537df9dca8 419 *id = packet.parameter[0];
GIPetrou 1:5f537df9dca8 420
GIPetrou 1:5f537df9dca8 421 #ifdef MX28_DEBUG
GIPetrou 1:5f537df9dca8 422 pc->printf("Get id: 0x%02X\r\n", *id);
GIPetrou 1:5f537df9dca8 423 #endif
GIPetrou 1:5f537df9dca8 424 }
GIPetrou 1:5f537df9dca8 425
GIPetrou 1:5f537df9dca8 426 return status;
GIPetrou 1:5f537df9dca8 427 }
GIPetrou 1:5f537df9dca8 428
GIPetrou 1:5f537df9dca8 429 uint8_t MX28::SetId(uint8_t servoId, uint8_t id, bool isRegWrite)
GIPetrou 1:5f537df9dca8 430 {
GIPetrou 1:5f537df9dca8 431 MX28_PROTOCOL_PACKET packet;
GIPetrou 1:5f537df9dca8 432
GIPetrou 1:5f537df9dca8 433 packet.servoId = servoId;
GIPetrou 1:5f537df9dca8 434 packet.length = 4;
GIPetrou 1:5f537df9dca8 435 packet.instructionErrorId = (isRegWrite == true) ? MX28_REG_WRITE : MX28_WRITE_DATA;
GIPetrou 1:5f537df9dca8 436 packet.parameter[0] = MX28_ID;
GIPetrou 1:5f537df9dca8 437 packet.parameter[1] = id;
GIPetrou 1:5f537df9dca8 438
GIPetrou 1:5f537df9dca8 439 #ifdef MX28_DEBUG
GIPetrou 1:5f537df9dca8 440 pc->printf("Set id: 0x%02X\r\n", id);
GIPetrou 1:5f537df9dca8 441 #endif
GIPetrou 1:5f537df9dca8 442
GIPetrou 1:5f537df9dca8 443 return CommunicatePacket(&packet);
GIPetrou 1:5f537df9dca8 444 }
GIPetrou 1:5f537df9dca8 445
GIPetrou 1:5f537df9dca8 446 uint8_t MX28::GetBaudRate(uint8_t servoId, int32_t *baudRate)
GIPetrou 1:5f537df9dca8 447 {
GIPetrou 1:5f537df9dca8 448 MX28_PROTOCOL_PACKET packet;
GIPetrou 1:5f537df9dca8 449
GIPetrou 1:5f537df9dca8 450 packet.servoId = servoId;
GIPetrou 1:5f537df9dca8 451 packet.length = 4;
GIPetrou 1:5f537df9dca8 452 packet.instructionErrorId = MX28_READ_DATA;
GIPetrou 1:5f537df9dca8 453 packet.parameter[0] = MX28_BAUD_RATE;
GIPetrou 1:5f537df9dca8 454 packet.parameter[1] = 1;
GIPetrou 1:5f537df9dca8 455
GIPetrou 1:5f537df9dca8 456 uint8_t status = CommunicatePacket(&packet);
GIPetrou 1:5f537df9dca8 457
GIPetrou 1:5f537df9dca8 458 if(status == MX28_ERRBIT_NONE)
GIPetrou 1:5f537df9dca8 459 {
GIPetrou 1:5f537df9dca8 460 if(packet.parameter[0] < 0xFA)
GIPetrou 1:5f537df9dca8 461 *baudRate = 2000000.0 / (double)(packet.parameter[0] + 1);
GIPetrou 1:5f537df9dca8 462 else if(packet.parameter[0] == 0xFA)
GIPetrou 1:5f537df9dca8 463 *baudRate = 2250000;
GIPetrou 1:5f537df9dca8 464 else if(packet.parameter[0] == 0xFB)
GIPetrou 1:5f537df9dca8 465 *baudRate = 2500000;
GIPetrou 1:5f537df9dca8 466 else
GIPetrou 1:5f537df9dca8 467 *baudRate = 3000000;
GIPetrou 1:5f537df9dca8 468
GIPetrou 1:5f537df9dca8 469 #ifdef MX28_DEBUG
GIPetrou 1:5f537df9dca8 470 pc->printf("Get baud rate: %d\r\n", *baudRate);
GIPetrou 1:5f537df9dca8 471 #endif
GIPetrou 1:5f537df9dca8 472 }
GIPetrou 1:5f537df9dca8 473
GIPetrou 1:5f537df9dca8 474 return status;
GIPetrou 1:5f537df9dca8 475 }
GIPetrou 1:5f537df9dca8 476
GIPetrou 1:5f537df9dca8 477 uint8_t MX28::SetBaudRate(uint8_t servoId, int32_t baudRate, bool isRegWrite)
GIPetrou 1:5f537df9dca8 478 {
GIPetrou 1:5f537df9dca8 479 MX28_PROTOCOL_PACKET packet;
GIPetrou 1:5f537df9dca8 480
GIPetrou 1:5f537df9dca8 481 uint8_t divisor = 0x00;
GIPetrou 1:5f537df9dca8 482
GIPetrou 1:5f537df9dca8 483 if(baudRate < 2250000)
GIPetrou 1:5f537df9dca8 484 divisor = (uint8_t)(2000000.0 / (double)baudRate) - 1;
GIPetrou 1:5f537df9dca8 485 else if(baudRate == 2250000)
GIPetrou 1:5f537df9dca8 486 divisor = 0xFA;
GIPetrou 1:5f537df9dca8 487 else if(baudRate == 2500000)
GIPetrou 1:5f537df9dca8 488 divisor = 0xFB;
GIPetrou 1:5f537df9dca8 489 else
GIPetrou 1:5f537df9dca8 490 divisor = 0xFC;
GIPetrou 1:5f537df9dca8 491
GIPetrou 1:5f537df9dca8 492 packet.servoId = servoId;
GIPetrou 1:5f537df9dca8 493 packet.length = 4;
GIPetrou 1:5f537df9dca8 494 packet.instructionErrorId = (isRegWrite == true) ? MX28_REG_WRITE : MX28_WRITE_DATA;
GIPetrou 1:5f537df9dca8 495 packet.parameter[0] = MX28_BAUD_RATE;
GIPetrou 1:5f537df9dca8 496 packet.parameter[1] = divisor;
GIPetrou 1:5f537df9dca8 497
GIPetrou 1:5f537df9dca8 498 #ifdef MX28_DEBUG
GIPetrou 1:5f537df9dca8 499 pc->printf("Set baudrate: 0x%02X\r\n", divisor);
GIPetrou 1:5f537df9dca8 500 #endif
GIPetrou 1:5f537df9dca8 501
GIPetrou 1:5f537df9dca8 502 return CommunicatePacket(&packet);
GIPetrou 1:5f537df9dca8 503 }
GIPetrou 1:5f537df9dca8 504
GIPetrou 1:5f537df9dca8 505 uint8_t MX28::GetReturnDelayTime(uint8_t servoId, uint8_t *returnDelayTime)
GIPetrou 1:5f537df9dca8 506 {
GIPetrou 1:5f537df9dca8 507 MX28_PROTOCOL_PACKET packet;
GIPetrou 1:5f537df9dca8 508
GIPetrou 1:5f537df9dca8 509 packet.servoId = servoId;
GIPetrou 1:5f537df9dca8 510 packet.length = 4;
GIPetrou 1:5f537df9dca8 511 packet.instructionErrorId = MX28_READ_DATA;
GIPetrou 1:5f537df9dca8 512 packet.parameter[0] = MX28_RETURN_DELAY_TIME;
GIPetrou 1:5f537df9dca8 513 packet.parameter[1] = 0x01;
GIPetrou 1:5f537df9dca8 514
GIPetrou 1:5f537df9dca8 515 uint8_t status = CommunicatePacket(&packet);
GIPetrou 1:5f537df9dca8 516
GIPetrou 1:5f537df9dca8 517 if(status == MX28_ERRBIT_NONE)
GIPetrou 1:5f537df9dca8 518 {
GIPetrou 1:5f537df9dca8 519 *returnDelayTime = packet.parameter[0];
GIPetrou 1:5f537df9dca8 520
GIPetrou 1:5f537df9dca8 521 #ifdef MX28_DEBUG
GIPetrou 1:5f537df9dca8 522 pc->printf("Get return delay time: 0x%02X\r\n", *returnDelayTime);
GIPetrou 1:5f537df9dca8 523 #endif
GIPetrou 1:5f537df9dca8 524 }
GIPetrou 1:5f537df9dca8 525
GIPetrou 1:5f537df9dca8 526 return status;
GIPetrou 1:5f537df9dca8 527 }
GIPetrou 1:5f537df9dca8 528
GIPetrou 1:5f537df9dca8 529 uint8_t MX28::SetReturnDelayTime(uint8_t servoId, uint8_t returnDelayTime, bool isRegWrite)
GIPetrou 1:5f537df9dca8 530 {
GIPetrou 1:5f537df9dca8 531 MX28_PROTOCOL_PACKET packet;
GIPetrou 1:5f537df9dca8 532
GIPetrou 1:5f537df9dca8 533 packet.servoId = servoId;
GIPetrou 1:5f537df9dca8 534 packet.length = 4;
GIPetrou 1:5f537df9dca8 535 packet.instructionErrorId = (isRegWrite == true) ? MX28_REG_WRITE : MX28_WRITE_DATA;
GIPetrou 1:5f537df9dca8 536 packet.parameter[0] = MX28_RETURN_DELAY_TIME;
GIPetrou 1:5f537df9dca8 537 packet.parameter[1] = returnDelayTime;
GIPetrou 1:5f537df9dca8 538
GIPetrou 1:5f537df9dca8 539 #ifdef MX28_DEBUG
GIPetrou 1:5f537df9dca8 540 pc->printf("Set return delay time: 0x%02X\r\n", returnDelayTime);
GIPetrou 1:5f537df9dca8 541 #endif
GIPetrou 1:5f537df9dca8 542
GIPetrou 1:5f537df9dca8 543 return CommunicatePacket(&packet);
GIPetrou 1:5f537df9dca8 544 }
GIPetrou 1:5f537df9dca8 545
GIPetrou 1:5f537df9dca8 546 uint8_t MX28::GetCWAngleLimit(uint8_t servoId, uint16_t *cwAngleLimit)
GIPetrou 1:5f537df9dca8 547 {
GIPetrou 1:5f537df9dca8 548 MX28_PROTOCOL_PACKET packet;
GIPetrou 1:5f537df9dca8 549
GIPetrou 1:5f537df9dca8 550 packet.servoId = servoId;
GIPetrou 1:5f537df9dca8 551 packet.length = 4;
GIPetrou 1:5f537df9dca8 552 packet.instructionErrorId = MX28_READ_DATA;
GIPetrou 1:5f537df9dca8 553 packet.parameter[0] = MX28_CW_ANGLE_LIMIT_L;
GIPetrou 1:5f537df9dca8 554 packet.parameter[1] = 0x02;
GIPetrou 1:5f537df9dca8 555
GIPetrou 1:5f537df9dca8 556 uint8_t status = CommunicatePacket(&packet);
GIPetrou 1:5f537df9dca8 557
GIPetrou 1:5f537df9dca8 558 if(status == MX28_ERRBIT_NONE)
GIPetrou 1:5f537df9dca8 559 {
GIPetrou 1:5f537df9dca8 560 *cwAngleLimit = Utilities::ConvertUInt8ArrayToUInt16(packet.parameter);
GIPetrou 1:5f537df9dca8 561
GIPetrou 1:5f537df9dca8 562 #ifdef MX28_DEBUG
GIPetrou 1:5f537df9dca8 563 pc->printf("Set CW angle limit: %hu\r\n", *cwAngleLimit);
GIPetrou 1:5f537df9dca8 564 #endif
GIPetrou 1:5f537df9dca8 565 }
GIPetrou 1:5f537df9dca8 566
GIPetrou 1:5f537df9dca8 567 return status;
GIPetrou 1:5f537df9dca8 568 }
GIPetrou 1:5f537df9dca8 569
GIPetrou 1:5f537df9dca8 570 uint8_t MX28::SetCWAngleLimit(uint8_t servoId, uint16_t cwAngleLimit, bool isRegWrite)
GIPetrou 1:5f537df9dca8 571 {
GIPetrou 1:5f537df9dca8 572 MX28_PROTOCOL_PACKET packet;
GIPetrou 1:5f537df9dca8 573
GIPetrou 1:5f537df9dca8 574 packet.servoId = servoId;
GIPetrou 1:5f537df9dca8 575 packet.length = 5;
GIPetrou 1:5f537df9dca8 576 packet.instructionErrorId = (isRegWrite == true) ? MX28_REG_WRITE : MX28_WRITE_DATA;
GIPetrou 1:5f537df9dca8 577 packet.parameter[0] = MX28_CW_ANGLE_LIMIT_L;
GIPetrou 1:5f537df9dca8 578 Utilities::ConvertInt16ToUInt8Array(cwAngleLimit, (uint8_t*)&(packet.parameter[1]));
GIPetrou 1:5f537df9dca8 579
GIPetrou 1:5f537df9dca8 580 #ifdef MX28_DEBUG
GIPetrou 1:5f537df9dca8 581 pc->printf("Set Cw angle limit: %hu\r\n", cwAngleLimit);
GIPetrou 1:5f537df9dca8 582 #endif
GIPetrou 1:5f537df9dca8 583
GIPetrou 1:5f537df9dca8 584 return CommunicatePacket(&packet);
GIPetrou 1:5f537df9dca8 585 }
GIPetrou 1:5f537df9dca8 586
GIPetrou 1:5f537df9dca8 587 uint8_t MX28::GetCCWAngleLimit(uint8_t servoId, uint16_t *ccwAngleLimit)
GIPetrou 1:5f537df9dca8 588 {
GIPetrou 1:5f537df9dca8 589 MX28_PROTOCOL_PACKET packet;
GIPetrou 1:5f537df9dca8 590
GIPetrou 1:5f537df9dca8 591 packet.servoId = servoId;
GIPetrou 1:5f537df9dca8 592 packet.length = 4;
GIPetrou 1:5f537df9dca8 593 packet.instructionErrorId = MX28_READ_DATA;
GIPetrou 1:5f537df9dca8 594 packet.parameter[0] = MX28_CCW_ANGLE_LIMIT_L;
GIPetrou 1:5f537df9dca8 595 packet.parameter[1] = 0x02;
GIPetrou 1:5f537df9dca8 596
GIPetrou 1:5f537df9dca8 597 uint8_t status = CommunicatePacket(&packet);
GIPetrou 1:5f537df9dca8 598
GIPetrou 1:5f537df9dca8 599 if(status == MX28_ERRBIT_NONE)
GIPetrou 1:5f537df9dca8 600 {
GIPetrou 1:5f537df9dca8 601 *ccwAngleLimit = Utilities::ConvertUInt8ArrayToUInt16(packet.parameter);
GIPetrou 1:5f537df9dca8 602
GIPetrou 1:5f537df9dca8 603 #ifdef MX28_DEBUG
GIPetrou 1:5f537df9dca8 604 pc->printf("Get CCW angle limit: %hu\r\n", *ccwAngleLimit);
GIPetrou 1:5f537df9dca8 605 #endif
GIPetrou 1:5f537df9dca8 606 }
GIPetrou 1:5f537df9dca8 607
GIPetrou 1:5f537df9dca8 608 return status;
GIPetrou 1:5f537df9dca8 609 }
GIPetrou 1:5f537df9dca8 610
GIPetrou 1:5f537df9dca8 611 uint8_t MX28::SetCCWAngleLimit(uint8_t servoId, uint16_t ccwAngleLimit, bool isRegWrite)
GIPetrou 1:5f537df9dca8 612 {
GIPetrou 1:5f537df9dca8 613 MX28_PROTOCOL_PACKET packet;
GIPetrou 1:5f537df9dca8 614
GIPetrou 1:5f537df9dca8 615 packet.servoId = servoId;
GIPetrou 1:5f537df9dca8 616 packet.length = 5;
GIPetrou 1:5f537df9dca8 617 packet.instructionErrorId = (isRegWrite == true) ? MX28_REG_WRITE : MX28_WRITE_DATA;
GIPetrou 1:5f537df9dca8 618 packet.parameter[0] = MX28_CCW_ANGLE_LIMIT_L;
GIPetrou 1:5f537df9dca8 619 Utilities::ConvertUInt16ToUInt8Array(ccwAngleLimit, (uint8_t*)&(packet.parameter[1]));
GIPetrou 1:5f537df9dca8 620
GIPetrou 1:5f537df9dca8 621 #ifdef MX28_DEBUG
GIPetrou 1:5f537df9dca8 622 pc->printf("Set CCW angle limit: %hu\r\n", ccwAngleLimit);
GIPetrou 1:5f537df9dca8 623 #endif
GIPetrou 1:5f537df9dca8 624
GIPetrou 1:5f537df9dca8 625 return CommunicatePacket(&packet);
GIPetrou 1:5f537df9dca8 626 }
GIPetrou 1:5f537df9dca8 627
GIPetrou 1:5f537df9dca8 628 uint8_t MX28::GetHighestTemperatureLimit(uint8_t servoId, uint8_t *highestTemperatureLimit)
GIPetrou 1:5f537df9dca8 629 {
GIPetrou 1:5f537df9dca8 630 MX28_PROTOCOL_PACKET packet;
GIPetrou 1:5f537df9dca8 631
GIPetrou 1:5f537df9dca8 632 packet.servoId = servoId;
GIPetrou 1:5f537df9dca8 633 packet.length = 4;
GIPetrou 1:5f537df9dca8 634 packet.instructionErrorId = MX28_READ_DATA;
GIPetrou 1:5f537df9dca8 635 packet.parameter[0] = MX28_UP_LIMIT_TEMPERATURE;
GIPetrou 1:5f537df9dca8 636 packet.parameter[1] = 0x01;
GIPetrou 1:5f537df9dca8 637
GIPetrou 1:5f537df9dca8 638 uint8_t status = CommunicatePacket(&packet);
GIPetrou 1:5f537df9dca8 639
GIPetrou 1:5f537df9dca8 640 if(status == MX28_ERRBIT_NONE)
GIPetrou 1:5f537df9dca8 641 {
GIPetrou 1:5f537df9dca8 642 *highestTemperatureLimit = packet.parameter[0];
GIPetrou 1:5f537df9dca8 643
GIPetrou 1:5f537df9dca8 644 #ifdef MX28_DEBUG
GIPetrou 1:5f537df9dca8 645 pc->printf("Get highest temperature limit: 0x%02X\r\n", *highestTemperatureLimit);
GIPetrou 1:5f537df9dca8 646 #endif
GIPetrou 1:5f537df9dca8 647 }
GIPetrou 1:5f537df9dca8 648
GIPetrou 1:5f537df9dca8 649 return status;
GIPetrou 1:5f537df9dca8 650 }
GIPetrou 1:5f537df9dca8 651
GIPetrou 1:5f537df9dca8 652 uint8_t MX28::SetHighestTemperatureLimit(uint8_t servoId, uint8_t highestTemperatureLimit, bool isRegWrite)
GIPetrou 1:5f537df9dca8 653 {
GIPetrou 1:5f537df9dca8 654 MX28_PROTOCOL_PACKET packet;
GIPetrou 1:5f537df9dca8 655
GIPetrou 1:5f537df9dca8 656 packet.servoId = servoId;
GIPetrou 1:5f537df9dca8 657 packet.length = 4;
GIPetrou 1:5f537df9dca8 658 packet.instructionErrorId = (isRegWrite == true) ? MX28_REG_WRITE : MX28_WRITE_DATA;
GIPetrou 1:5f537df9dca8 659 packet.parameter[0] = MX28_UP_LIMIT_TEMPERATURE;
GIPetrou 1:5f537df9dca8 660 packet.parameter[1] = highestTemperatureLimit;
GIPetrou 1:5f537df9dca8 661
GIPetrou 1:5f537df9dca8 662 #ifdef MX28_DEBUG
GIPetrou 1:5f537df9dca8 663 pc->printf("Set highest temperature limit: 0x%02X\r\n", highestTemperatureLimit);
GIPetrou 1:5f537df9dca8 664 #endif
GIPetrou 1:5f537df9dca8 665
GIPetrou 1:5f537df9dca8 666 return CommunicatePacket(&packet);
GIPetrou 1:5f537df9dca8 667 }
GIPetrou 1:5f537df9dca8 668
GIPetrou 1:5f537df9dca8 669 uint8_t MX28::GetLowestVoltageLimit(uint8_t servoId, uint8_t *lowestVoltageLimit)
GIPetrou 1:5f537df9dca8 670 {
GIPetrou 1:5f537df9dca8 671 MX28_PROTOCOL_PACKET packet;
GIPetrou 1:5f537df9dca8 672
GIPetrou 1:5f537df9dca8 673 packet.servoId = servoId;
GIPetrou 1:5f537df9dca8 674 packet.length = 4;
GIPetrou 1:5f537df9dca8 675 packet.instructionErrorId = MX28_READ_DATA;
GIPetrou 1:5f537df9dca8 676 packet.parameter[0] = MX28_DOWN_LIMIT_VOLTAGE;
GIPetrou 1:5f537df9dca8 677 packet.parameter[1] = 0x01;
GIPetrou 1:5f537df9dca8 678
GIPetrou 1:5f537df9dca8 679 uint8_t status = CommunicatePacket(&packet);
GIPetrou 1:5f537df9dca8 680
GIPetrou 1:5f537df9dca8 681 if(status == MX28_ERRBIT_NONE)
GIPetrou 1:5f537df9dca8 682 {
GIPetrou 1:5f537df9dca8 683 *lowestVoltageLimit = packet.parameter[0];
GIPetrou 1:5f537df9dca8 684
GIPetrou 1:5f537df9dca8 685 #ifdef MX28_DEBUG
GIPetrou 1:5f537df9dca8 686 pc->printf("Get lowest voltage limit: 0x%02X\r\n", *lowestVoltageLimit);
GIPetrou 1:5f537df9dca8 687 #endif
GIPetrou 1:5f537df9dca8 688 }
GIPetrou 1:5f537df9dca8 689
GIPetrou 1:5f537df9dca8 690 return status;
GIPetrou 1:5f537df9dca8 691 }
GIPetrou 1:5f537df9dca8 692
GIPetrou 1:5f537df9dca8 693 uint8_t MX28::SetLowestVoltageLimit(uint8_t servoId, uint8_t lowestVoltageLimit, bool isRegWrite)
GIPetrou 1:5f537df9dca8 694 {
GIPetrou 1:5f537df9dca8 695 MX28_PROTOCOL_PACKET packet;
GIPetrou 1:5f537df9dca8 696
GIPetrou 1:5f537df9dca8 697 packet.servoId = servoId;
GIPetrou 1:5f537df9dca8 698 packet.length = 4;
GIPetrou 1:5f537df9dca8 699 packet.instructionErrorId = (isRegWrite == true) ? MX28_REG_WRITE : MX28_WRITE_DATA;
GIPetrou 1:5f537df9dca8 700 packet.parameter[0] = MX28_DOWN_LIMIT_VOLTAGE;
GIPetrou 1:5f537df9dca8 701 packet.parameter[1] = lowestVoltageLimit;
GIPetrou 1:5f537df9dca8 702
GIPetrou 1:5f537df9dca8 703 #ifdef MX28_DEBUG
GIPetrou 1:5f537df9dca8 704 pc->printf("Set lowest voltage limit: 0x%02X\r\n", lowestVoltageLimit);
GIPetrou 1:5f537df9dca8 705 #endif
GIPetrou 1:5f537df9dca8 706
GIPetrou 1:5f537df9dca8 707 return CommunicatePacket(&packet);
GIPetrou 1:5f537df9dca8 708 }
GIPetrou 1:5f537df9dca8 709
GIPetrou 1:5f537df9dca8 710 uint8_t MX28::GetHighestVoltageLimit(uint8_t servoId, uint8_t *highestVoltageLimit)
GIPetrou 1:5f537df9dca8 711 {
GIPetrou 1:5f537df9dca8 712 MX28_PROTOCOL_PACKET packet;
GIPetrou 1:5f537df9dca8 713
GIPetrou 1:5f537df9dca8 714 packet.servoId = servoId;
GIPetrou 1:5f537df9dca8 715 packet.length = 4;
GIPetrou 1:5f537df9dca8 716 packet.instructionErrorId = MX28_READ_DATA;
GIPetrou 1:5f537df9dca8 717 packet.parameter[0] = MX28_UP_LIMIT_VOLTAGE;
GIPetrou 1:5f537df9dca8 718 packet.parameter[1] = 0x01;
GIPetrou 1:5f537df9dca8 719
GIPetrou 1:5f537df9dca8 720 uint8_t status = CommunicatePacket(&packet);
GIPetrou 1:5f537df9dca8 721
GIPetrou 1:5f537df9dca8 722 if(status == MX28_ERRBIT_NONE)
GIPetrou 1:5f537df9dca8 723 {
GIPetrou 1:5f537df9dca8 724 *highestVoltageLimit = packet.parameter[0];
GIPetrou 1:5f537df9dca8 725
GIPetrou 1:5f537df9dca8 726 #ifdef MX28_DEBUG
GIPetrou 1:5f537df9dca8 727 pc->printf("Get highest voltage limit: 0x%02X\r\n", *highestVoltageLimit);
GIPetrou 1:5f537df9dca8 728 #endif
GIPetrou 1:5f537df9dca8 729 }
GIPetrou 1:5f537df9dca8 730
GIPetrou 1:5f537df9dca8 731 return status;
GIPetrou 1:5f537df9dca8 732 }
GIPetrou 1:5f537df9dca8 733
GIPetrou 1:5f537df9dca8 734 uint8_t MX28::SetHighestVoltageLimit(uint8_t servoId, uint8_t highestVoltageLimit, bool isRegWrite)
GIPetrou 1:5f537df9dca8 735 {
GIPetrou 1:5f537df9dca8 736 MX28_PROTOCOL_PACKET packet;
GIPetrou 1:5f537df9dca8 737
GIPetrou 1:5f537df9dca8 738 packet.servoId = servoId;
GIPetrou 1:5f537df9dca8 739 packet.length = 4;
GIPetrou 1:5f537df9dca8 740 packet.instructionErrorId = (isRegWrite == true) ? MX28_REG_WRITE : MX28_WRITE_DATA;
GIPetrou 1:5f537df9dca8 741 packet.parameter[0] = MX28_UP_LIMIT_VOLTAGE;
GIPetrou 1:5f537df9dca8 742 packet.parameter[1] = highestVoltageLimit;
GIPetrou 1:5f537df9dca8 743
GIPetrou 1:5f537df9dca8 744 #ifdef MX28_DEBUG
GIPetrou 1:5f537df9dca8 745 pc->printf("Set highest voltage limit: 0x%02X\r\n", highestVoltageLimit);
GIPetrou 1:5f537df9dca8 746 #endif
GIPetrou 1:5f537df9dca8 747
GIPetrou 1:5f537df9dca8 748 return CommunicatePacket(&packet);
GIPetrou 1:5f537df9dca8 749 }
GIPetrou 1:5f537df9dca8 750
GIPetrou 1:5f537df9dca8 751 uint8_t MX28::GetMaxTorque(uint8_t servoId, uint16_t *maxTorque)
GIPetrou 1:5f537df9dca8 752 {
GIPetrou 1:5f537df9dca8 753 MX28_PROTOCOL_PACKET packet;
GIPetrou 1:5f537df9dca8 754
GIPetrou 1:5f537df9dca8 755 packet.servoId = servoId;
GIPetrou 1:5f537df9dca8 756 packet.length = 4;
GIPetrou 1:5f537df9dca8 757 packet.instructionErrorId = MX28_READ_DATA;
GIPetrou 1:5f537df9dca8 758 packet.parameter[0] = MX28_MAX_TORQUE_L;
GIPetrou 1:5f537df9dca8 759 packet.parameter[1] = 0x02;
GIPetrou 1:5f537df9dca8 760
GIPetrou 1:5f537df9dca8 761 uint8_t status = CommunicatePacket(&packet);
GIPetrou 1:5f537df9dca8 762
GIPetrou 1:5f537df9dca8 763 if(status == MX28_ERRBIT_NONE)
GIPetrou 1:5f537df9dca8 764 {
GIPetrou 1:5f537df9dca8 765 *maxTorque = Utilities::ConvertUInt8ArrayToUInt16(packet.parameter);
GIPetrou 1:5f537df9dca8 766
GIPetrou 1:5f537df9dca8 767 #ifdef MX28_DEBUG
GIPetrou 1:5f537df9dca8 768 pc->printf("Get max torque: %hu\r\n", *maxTorque);
GIPetrou 1:5f537df9dca8 769 #endif
GIPetrou 1:5f537df9dca8 770 }
GIPetrou 1:5f537df9dca8 771
GIPetrou 1:5f537df9dca8 772 return status;
GIPetrou 1:5f537df9dca8 773 }
GIPetrou 1:5f537df9dca8 774
GIPetrou 1:5f537df9dca8 775 uint8_t MX28::SetMaxTorque(uint8_t servoId, uint16_t maxTorque, bool isRegWrite)
GIPetrou 1:5f537df9dca8 776 {
GIPetrou 1:5f537df9dca8 777 MX28_PROTOCOL_PACKET packet;
GIPetrou 1:5f537df9dca8 778
GIPetrou 1:5f537df9dca8 779 packet.servoId = servoId;
GIPetrou 1:5f537df9dca8 780 packet.length = 5;
GIPetrou 1:5f537df9dca8 781 packet.instructionErrorId = (isRegWrite == true) ? MX28_REG_WRITE : MX28_WRITE_DATA;
GIPetrou 1:5f537df9dca8 782 packet.parameter[0] = MX28_MAX_TORQUE_L;
GIPetrou 1:5f537df9dca8 783 Utilities::ConvertUInt16ToUInt8Array(maxTorque, (uint8_t*)&(packet.parameter[1]));
GIPetrou 1:5f537df9dca8 784
GIPetrou 1:5f537df9dca8 785 #ifdef MX28_DEBUG
GIPetrou 1:5f537df9dca8 786 pc->printf("Set max torque: %hu\r\n", maxTorque);
GIPetrou 1:5f537df9dca8 787 #endif
GIPetrou 1:5f537df9dca8 788
GIPetrou 1:5f537df9dca8 789 return CommunicatePacket(&packet);
GIPetrou 1:5f537df9dca8 790 }
GIPetrou 1:5f537df9dca8 791
GIPetrou 1:5f537df9dca8 792 uint8_t MX28::GetStatusReturnLevel(uint8_t servoId, uint8_t *statusReturnLevel)
GIPetrou 1:5f537df9dca8 793 {
GIPetrou 1:5f537df9dca8 794 MX28_PROTOCOL_PACKET packet;
GIPetrou 1:5f537df9dca8 795
GIPetrou 1:5f537df9dca8 796 packet.servoId = servoId;
GIPetrou 1:5f537df9dca8 797 packet.length = 4;
GIPetrou 1:5f537df9dca8 798 packet.instructionErrorId = MX28_READ_DATA;
GIPetrou 1:5f537df9dca8 799 packet.parameter[0] = MX28_STATUS_RETURN_LEVEL;
GIPetrou 1:5f537df9dca8 800 packet.parameter[1] = 0x01;
GIPetrou 1:5f537df9dca8 801
GIPetrou 1:5f537df9dca8 802 uint8_t status = CommunicatePacket(&packet);
GIPetrou 1:5f537df9dca8 803
GIPetrou 1:5f537df9dca8 804 if(status == MX28_ERRBIT_NONE)
GIPetrou 1:5f537df9dca8 805 {
GIPetrou 1:5f537df9dca8 806 *statusReturnLevel = packet.parameter[0];
GIPetrou 1:5f537df9dca8 807
GIPetrou 1:5f537df9dca8 808 #ifdef MX28_DEBUG
GIPetrou 1:5f537df9dca8 809 pc->printf("Get status return level: 0x%02X\r\n", *statusReturnLevel);
GIPetrou 1:5f537df9dca8 810 #endif
GIPetrou 1:5f537df9dca8 811 }
GIPetrou 1:5f537df9dca8 812
GIPetrou 1:5f537df9dca8 813 return status;
GIPetrou 1:5f537df9dca8 814 }
GIPetrou 1:5f537df9dca8 815
GIPetrou 1:5f537df9dca8 816 uint8_t MX28::SetStatusReturnLevel(uint8_t servoId, uint8_t statusReturnLevel, bool isRegWrite)
GIPetrou 1:5f537df9dca8 817 {
GIPetrou 1:5f537df9dca8 818 MX28_PROTOCOL_PACKET packet;
GIPetrou 1:5f537df9dca8 819
GIPetrou 1:5f537df9dca8 820 packet.servoId = servoId;
GIPetrou 1:5f537df9dca8 821 packet.length = 4;
GIPetrou 1:5f537df9dca8 822 packet.instructionErrorId = (isRegWrite == true) ? MX28_REG_WRITE : MX28_WRITE_DATA;
GIPetrou 1:5f537df9dca8 823 packet.parameter[0] = MX28_STATUS_RETURN_LEVEL;
GIPetrou 1:5f537df9dca8 824 packet.parameter[1] = statusReturnLevel;
GIPetrou 1:5f537df9dca8 825
GIPetrou 1:5f537df9dca8 826 #ifdef MX28_DEBUG
GIPetrou 1:5f537df9dca8 827 pc->printf("Set status return level: 0x%02X\r\n", statusReturnLevel);
GIPetrou 1:5f537df9dca8 828 #endif
GIPetrou 1:5f537df9dca8 829
GIPetrou 1:5f537df9dca8 830 return CommunicatePacket(&packet);
GIPetrou 1:5f537df9dca8 831 }
GIPetrou 1:5f537df9dca8 832
GIPetrou 1:5f537df9dca8 833 uint8_t MX28::GetAlarmLED(uint8_t servoId, uint8_t *alarmLED)
GIPetrou 1:5f537df9dca8 834 {
GIPetrou 1:5f537df9dca8 835 MX28_PROTOCOL_PACKET packet;
GIPetrou 1:5f537df9dca8 836
GIPetrou 1:5f537df9dca8 837 packet.servoId = servoId;
GIPetrou 1:5f537df9dca8 838 packet.length = 4;
GIPetrou 1:5f537df9dca8 839 packet.instructionErrorId = MX28_READ_DATA;
GIPetrou 1:5f537df9dca8 840 packet.parameter[0] = MX28_ALARM_LED;
GIPetrou 1:5f537df9dca8 841 packet.parameter[1] = 0x01;
GIPetrou 1:5f537df9dca8 842
GIPetrou 1:5f537df9dca8 843 uint8_t status = CommunicatePacket(&packet);
GIPetrou 1:5f537df9dca8 844
GIPetrou 1:5f537df9dca8 845 if(status == MX28_ERRBIT_NONE)
GIPetrou 1:5f537df9dca8 846 {
GIPetrou 1:5f537df9dca8 847 *alarmLED = packet.parameter[0];
GIPetrou 1:5f537df9dca8 848
GIPetrou 1:5f537df9dca8 849 #ifdef MX28_DEBUG
GIPetrou 1:5f537df9dca8 850 pc->printf("Get alarm LED: 0x%02X\r\n", *alarmLED);
GIPetrou 1:5f537df9dca8 851 #endif
GIPetrou 1:5f537df9dca8 852 }
GIPetrou 1:5f537df9dca8 853
GIPetrou 1:5f537df9dca8 854 return status;
GIPetrou 1:5f537df9dca8 855 }
GIPetrou 1:5f537df9dca8 856
GIPetrou 1:5f537df9dca8 857 uint8_t MX28::SetAlarmLED(uint8_t servoId, uint8_t alarmLED, bool isRegWrite)
GIPetrou 1:5f537df9dca8 858 {
GIPetrou 1:5f537df9dca8 859 MX28_PROTOCOL_PACKET packet;
GIPetrou 1:5f537df9dca8 860
GIPetrou 1:5f537df9dca8 861 packet.servoId = servoId;
GIPetrou 1:5f537df9dca8 862 packet.length = 4;
GIPetrou 1:5f537df9dca8 863 packet.instructionErrorId = (isRegWrite == true) ? MX28_REG_WRITE : MX28_WRITE_DATA;
GIPetrou 1:5f537df9dca8 864 packet.parameter[0] = MX28_ALARM_LED;
GIPetrou 1:5f537df9dca8 865 packet.parameter[1] = alarmLED;
GIPetrou 1:5f537df9dca8 866
GIPetrou 1:5f537df9dca8 867 #ifdef MX28_DEBUG
GIPetrou 1:5f537df9dca8 868 pc->printf("Set alarm LED: 0x%02X\r\n", alarmLED);
GIPetrou 1:5f537df9dca8 869 #endif
GIPetrou 1:5f537df9dca8 870
GIPetrou 1:5f537df9dca8 871 return CommunicatePacket(&packet);
GIPetrou 1:5f537df9dca8 872 }
GIPetrou 1:5f537df9dca8 873
GIPetrou 1:5f537df9dca8 874 uint8_t MX28::GetAlarmShutdown(uint8_t servoId, uint8_t *alarmShutdown)
GIPetrou 1:5f537df9dca8 875 {
GIPetrou 1:5f537df9dca8 876 MX28_PROTOCOL_PACKET packet;
GIPetrou 1:5f537df9dca8 877
GIPetrou 1:5f537df9dca8 878 packet.servoId = servoId;
GIPetrou 1:5f537df9dca8 879 packet.length = 4;
GIPetrou 1:5f537df9dca8 880 packet.instructionErrorId = MX28_READ_DATA;
GIPetrou 1:5f537df9dca8 881 packet.parameter[0] = MX28_ALARM_SHUTDOWN;
GIPetrou 1:5f537df9dca8 882 packet.parameter[1] = 0x01;
GIPetrou 1:5f537df9dca8 883
GIPetrou 1:5f537df9dca8 884 uint8_t status = CommunicatePacket(&packet);
GIPetrou 1:5f537df9dca8 885
GIPetrou 1:5f537df9dca8 886 if(status == MX28_ERRBIT_NONE)
GIPetrou 1:5f537df9dca8 887 {
GIPetrou 1:5f537df9dca8 888 *alarmShutdown = packet.parameter[0];
GIPetrou 1:5f537df9dca8 889
GIPetrou 1:5f537df9dca8 890 #ifdef MX28_DEBUG
GIPetrou 1:5f537df9dca8 891 pc->printf("Get alarm shutdown: 0x%02X\r\n", *alarmShutdown);
GIPetrou 1:5f537df9dca8 892 #endif
GIPetrou 1:5f537df9dca8 893 }
GIPetrou 1:5f537df9dca8 894
GIPetrou 1:5f537df9dca8 895 return status;
GIPetrou 1:5f537df9dca8 896 }
GIPetrou 1:5f537df9dca8 897
GIPetrou 1:5f537df9dca8 898 uint8_t MX28::SetAlarmShutdown(uint8_t servoId, uint8_t alarmShutdown, bool isRegWrite)
GIPetrou 1:5f537df9dca8 899 {
GIPetrou 1:5f537df9dca8 900 MX28_PROTOCOL_PACKET packet;
GIPetrou 1:5f537df9dca8 901
GIPetrou 1:5f537df9dca8 902 packet.servoId = servoId;
GIPetrou 1:5f537df9dca8 903 packet.length = 4;
GIPetrou 1:5f537df9dca8 904 packet.instructionErrorId = (isRegWrite == true) ? MX28_REG_WRITE : MX28_WRITE_DATA;
GIPetrou 1:5f537df9dca8 905 packet.parameter[0] = MX28_ALARM_SHUTDOWN;
GIPetrou 1:5f537df9dca8 906 packet.parameter[1] = alarmShutdown;
GIPetrou 1:5f537df9dca8 907
GIPetrou 1:5f537df9dca8 908 #ifdef MX28_DEBUG
GIPetrou 1:5f537df9dca8 909 pc->printf("Set alarm shutdown: 0x%02X\r\n", alarmShutdown);
GIPetrou 1:5f537df9dca8 910 #endif
GIPetrou 1:5f537df9dca8 911
GIPetrou 1:5f537df9dca8 912 return CommunicatePacket(&packet);
GIPetrou 1:5f537df9dca8 913 }
GIPetrou 1:5f537df9dca8 914
GIPetrou 1:5f537df9dca8 915 uint8_t MX28::GetEnableTorque(uint8_t servoId, uint8_t *enableTorque)
GIPetrou 1:5f537df9dca8 916 {
GIPetrou 1:5f537df9dca8 917 MX28_PROTOCOL_PACKET packet;
GIPetrou 1:5f537df9dca8 918
GIPetrou 1:5f537df9dca8 919 packet.servoId = servoId;
GIPetrou 1:5f537df9dca8 920 packet.length = 4;
GIPetrou 1:5f537df9dca8 921 packet.instructionErrorId = MX28_READ_DATA;
GIPetrou 1:5f537df9dca8 922 packet.parameter[0] = MX28_TORQUE_ENABLE;
GIPetrou 1:5f537df9dca8 923 packet.parameter[1] = 0x01;
GIPetrou 1:5f537df9dca8 924
GIPetrou 1:5f537df9dca8 925 uint8_t status = CommunicatePacket(&packet);
GIPetrou 1:5f537df9dca8 926
GIPetrou 1:5f537df9dca8 927 if(status == MX28_ERRBIT_NONE)
GIPetrou 1:5f537df9dca8 928 {
GIPetrou 1:5f537df9dca8 929 *enableTorque = packet.parameter[0];
GIPetrou 1:5f537df9dca8 930
GIPetrou 1:5f537df9dca8 931 #ifdef MX28_DEBUG
GIPetrou 1:5f537df9dca8 932 pc->printf("Get enable torque: 0x%02X\r\n", *enableTorque);
GIPetrou 1:5f537df9dca8 933 #endif
GIPetrou 1:5f537df9dca8 934 }
GIPetrou 1:5f537df9dca8 935
GIPetrou 1:5f537df9dca8 936 return status;
GIPetrou 1:5f537df9dca8 937 }
GIPetrou 1:5f537df9dca8 938
GIPetrou 1:5f537df9dca8 939 uint8_t MX28::SetEnableTorque(uint8_t servoId, uint8_t enableTorque, bool isRegWrite)
GIPetrou 1:5f537df9dca8 940 {
GIPetrou 1:5f537df9dca8 941 MX28_PROTOCOL_PACKET packet;
GIPetrou 1:5f537df9dca8 942
GIPetrou 1:5f537df9dca8 943 packet.servoId = servoId;
GIPetrou 1:5f537df9dca8 944 packet.length = 4;
GIPetrou 1:5f537df9dca8 945 packet.instructionErrorId = (isRegWrite == true) ? MX28_REG_WRITE : MX28_WRITE_DATA;
GIPetrou 1:5f537df9dca8 946 packet.parameter[0] = MX28_TORQUE_ENABLE;
GIPetrou 1:5f537df9dca8 947 packet.parameter[1] = enableTorque;
GIPetrou 1:5f537df9dca8 948
GIPetrou 1:5f537df9dca8 949 #ifdef MX28_DEBUG
GIPetrou 1:5f537df9dca8 950 pc->printf("Set enable torque: 0x%02X\r\n", enableTorque);
GIPetrou 1:5f537df9dca8 951 #endif
GIPetrou 1:5f537df9dca8 952
GIPetrou 1:5f537df9dca8 953 return CommunicatePacket(&packet);
GIPetrou 1:5f537df9dca8 954 }
GIPetrou 1:5f537df9dca8 955
GIPetrou 1:5f537df9dca8 956 uint8_t MX28::GetEnableLED(uint8_t servoId, uint8_t *enableLED)
GIPetrou 1:5f537df9dca8 957 {
GIPetrou 1:5f537df9dca8 958 MX28_PROTOCOL_PACKET packet;
GIPetrou 1:5f537df9dca8 959
GIPetrou 1:5f537df9dca8 960 packet.servoId = servoId;
GIPetrou 1:5f537df9dca8 961 packet.length = 4;
GIPetrou 1:5f537df9dca8 962 packet.instructionErrorId = MX28_READ_DATA;
GIPetrou 1:5f537df9dca8 963 packet.parameter[0] = MX28_LED_ENABLE;
GIPetrou 1:5f537df9dca8 964 packet.parameter[1] = 0x01;
GIPetrou 1:5f537df9dca8 965
GIPetrou 1:5f537df9dca8 966 uint8_t status = CommunicatePacket(&packet);
GIPetrou 1:5f537df9dca8 967
GIPetrou 1:5f537df9dca8 968 if(status == MX28_ERRBIT_NONE)
GIPetrou 1:5f537df9dca8 969 {
GIPetrou 1:5f537df9dca8 970 *enableLED = packet.parameter[0];
GIPetrou 1:5f537df9dca8 971
GIPetrou 1:5f537df9dca8 972 #ifdef MX28_DEBUG
GIPetrou 1:5f537df9dca8 973 pc->printf("Get enable LED: 0x%02X\r\n", *enableLED);
GIPetrou 1:5f537df9dca8 974 #endif
GIPetrou 1:5f537df9dca8 975 }
GIPetrou 1:5f537df9dca8 976
GIPetrou 1:5f537df9dca8 977 return status;
GIPetrou 1:5f537df9dca8 978 }
GIPetrou 1:5f537df9dca8 979
GIPetrou 1:5f537df9dca8 980 uint8_t MX28::SetEnableLED(uint8_t servoId, uint8_t enableLED, bool isRegWrite)
GIPetrou 1:5f537df9dca8 981 {
GIPetrou 1:5f537df9dca8 982 MX28_PROTOCOL_PACKET packet;
GIPetrou 1:5f537df9dca8 983
GIPetrou 1:5f537df9dca8 984 packet.servoId = servoId;
GIPetrou 1:5f537df9dca8 985 packet.length = 4;
GIPetrou 1:5f537df9dca8 986 packet.instructionErrorId = (isRegWrite == true) ? MX28_REG_WRITE : MX28_WRITE_DATA;
GIPetrou 1:5f537df9dca8 987 packet.parameter[0] = MX28_LED_ENABLE;
GIPetrou 1:5f537df9dca8 988 packet.parameter[1] = enableLED;
GIPetrou 1:5f537df9dca8 989
GIPetrou 1:5f537df9dca8 990 #ifdef MX28_DEBUG
GIPetrou 1:5f537df9dca8 991 pc->printf("Set enable LED: 0x%02X\r\n", enableLED);
GIPetrou 1:5f537df9dca8 992 #endif
GIPetrou 1:5f537df9dca8 993
GIPetrou 1:5f537df9dca8 994 return CommunicatePacket(&packet);
GIPetrou 1:5f537df9dca8 995 }
GIPetrou 1:5f537df9dca8 996
GIPetrou 1:5f537df9dca8 997 uint8_t MX28::GetPGain(uint8_t servoId, uint8_t *pGain)
GIPetrou 1:5f537df9dca8 998 {
GIPetrou 1:5f537df9dca8 999 MX28_PROTOCOL_PACKET packet;
GIPetrou 1:5f537df9dca8 1000
GIPetrou 1:5f537df9dca8 1001 packet.servoId = servoId;
GIPetrou 1:5f537df9dca8 1002 packet.length = 4;
GIPetrou 1:5f537df9dca8 1003 packet.instructionErrorId = MX28_READ_DATA;
GIPetrou 1:5f537df9dca8 1004 packet.parameter[0] = MX28_P_GAIN;
GIPetrou 1:5f537df9dca8 1005 packet.parameter[1] = 0x01;
GIPetrou 1:5f537df9dca8 1006
GIPetrou 1:5f537df9dca8 1007 uint8_t status = CommunicatePacket(&packet);
GIPetrou 1:5f537df9dca8 1008
GIPetrou 1:5f537df9dca8 1009 if(status == MX28_ERRBIT_NONE)
GIPetrou 1:5f537df9dca8 1010 {
GIPetrou 1:5f537df9dca8 1011 *pGain = packet.parameter[0];
GIPetrou 1:5f537df9dca8 1012
GIPetrou 1:5f537df9dca8 1013 #ifdef MX28_DEBUG
GIPetrou 1:5f537df9dca8 1014 pc->printf("Get P gain: 0x%02X\r\n", *pGain);
GIPetrou 1:5f537df9dca8 1015 #endif
GIPetrou 1:5f537df9dca8 1016 }
GIPetrou 1:5f537df9dca8 1017
GIPetrou 1:5f537df9dca8 1018 return status;
GIPetrou 1:5f537df9dca8 1019 }
GIPetrou 1:5f537df9dca8 1020
GIPetrou 1:5f537df9dca8 1021 uint8_t MX28::SetPGain(uint8_t servoId, uint8_t pGain, bool isRegWrite)
GIPetrou 1:5f537df9dca8 1022 {
GIPetrou 1:5f537df9dca8 1023 MX28_PROTOCOL_PACKET packet;
GIPetrou 1:5f537df9dca8 1024
GIPetrou 1:5f537df9dca8 1025 packet.servoId = servoId;
GIPetrou 1:5f537df9dca8 1026 packet.length = 4;
GIPetrou 1:5f537df9dca8 1027 packet.instructionErrorId = (isRegWrite == true) ? MX28_REG_WRITE : MX28_WRITE_DATA;
GIPetrou 1:5f537df9dca8 1028 packet.parameter[0] = MX28_P_GAIN;
GIPetrou 1:5f537df9dca8 1029 packet.parameter[1] = pGain;
GIPetrou 1:5f537df9dca8 1030
GIPetrou 1:5f537df9dca8 1031 #ifdef MX28_DEBUG
GIPetrou 1:5f537df9dca8 1032 pc->printf("Set P gain: 0x%02X\r\n", pGain);
GIPetrou 1:5f537df9dca8 1033 #endif
GIPetrou 1:5f537df9dca8 1034
GIPetrou 1:5f537df9dca8 1035 return CommunicatePacket(&packet);
GIPetrou 1:5f537df9dca8 1036 }
GIPetrou 1:5f537df9dca8 1037
GIPetrou 1:5f537df9dca8 1038 uint8_t MX28::GetIGain(uint8_t servoId, uint8_t *iGain)
GIPetrou 1:5f537df9dca8 1039 {
GIPetrou 1:5f537df9dca8 1040 MX28_PROTOCOL_PACKET packet;
GIPetrou 1:5f537df9dca8 1041
GIPetrou 1:5f537df9dca8 1042 packet.servoId = servoId;
GIPetrou 1:5f537df9dca8 1043 packet.length = 4;
GIPetrou 1:5f537df9dca8 1044 packet.instructionErrorId = MX28_READ_DATA;
GIPetrou 1:5f537df9dca8 1045 packet.parameter[0] = MX28_I_GAIN;
GIPetrou 1:5f537df9dca8 1046 packet.parameter[1] = 0x01;
GIPetrou 1:5f537df9dca8 1047
GIPetrou 1:5f537df9dca8 1048 uint8_t status = CommunicatePacket(&packet);
GIPetrou 1:5f537df9dca8 1049
GIPetrou 1:5f537df9dca8 1050 if(status == MX28_ERRBIT_NONE)
GIPetrou 1:5f537df9dca8 1051 {
GIPetrou 1:5f537df9dca8 1052 *iGain = packet.parameter[0];
GIPetrou 1:5f537df9dca8 1053
GIPetrou 1:5f537df9dca8 1054 #ifdef MX28_DEBUG
GIPetrou 1:5f537df9dca8 1055 pc->printf("Get I gain: 0x%02X\r\n", *iGain);
GIPetrou 1:5f537df9dca8 1056 #endif
GIPetrou 1:5f537df9dca8 1057 }
GIPetrou 1:5f537df9dca8 1058
GIPetrou 1:5f537df9dca8 1059 return status;
GIPetrou 1:5f537df9dca8 1060 }
GIPetrou 1:5f537df9dca8 1061
GIPetrou 1:5f537df9dca8 1062 uint8_t MX28::SetIGain(uint8_t servoId, uint8_t iGain, bool isRegWrite)
GIPetrou 1:5f537df9dca8 1063 {
GIPetrou 1:5f537df9dca8 1064 MX28_PROTOCOL_PACKET packet;
GIPetrou 1:5f537df9dca8 1065
GIPetrou 1:5f537df9dca8 1066 packet.servoId = servoId;
GIPetrou 1:5f537df9dca8 1067 packet.length = 4;
GIPetrou 1:5f537df9dca8 1068 packet.instructionErrorId = (isRegWrite == true) ? MX28_REG_WRITE : MX28_WRITE_DATA;
GIPetrou 1:5f537df9dca8 1069 packet.parameter[0] = MX28_I_GAIN;
GIPetrou 1:5f537df9dca8 1070 packet.parameter[1] = iGain;
GIPetrou 1:5f537df9dca8 1071
GIPetrou 1:5f537df9dca8 1072 #ifdef MX28_DEBUG
GIPetrou 1:5f537df9dca8 1073 pc->printf("Set I gain: 0x%02X\r\n", iGain);
GIPetrou 1:5f537df9dca8 1074 #endif
GIPetrou 1:5f537df9dca8 1075
GIPetrou 1:5f537df9dca8 1076 return CommunicatePacket(&packet);
GIPetrou 1:5f537df9dca8 1077 }
GIPetrou 1:5f537df9dca8 1078
GIPetrou 1:5f537df9dca8 1079 uint8_t MX28::GetDGain(uint8_t servoId, uint8_t *dGain)
GIPetrou 1:5f537df9dca8 1080 {
GIPetrou 1:5f537df9dca8 1081 MX28_PROTOCOL_PACKET packet;
GIPetrou 1:5f537df9dca8 1082
GIPetrou 1:5f537df9dca8 1083 packet.servoId = servoId;
GIPetrou 1:5f537df9dca8 1084 packet.length = 4;
GIPetrou 1:5f537df9dca8 1085 packet.instructionErrorId = MX28_READ_DATA;
GIPetrou 1:5f537df9dca8 1086 packet.parameter[0] = MX28_D_GAIN;
GIPetrou 1:5f537df9dca8 1087 packet.parameter[1] = 0x01;
GIPetrou 1:5f537df9dca8 1088
GIPetrou 1:5f537df9dca8 1089 uint8_t status = CommunicatePacket(&packet);
GIPetrou 1:5f537df9dca8 1090
GIPetrou 1:5f537df9dca8 1091 if(status == MX28_ERRBIT_NONE)
GIPetrou 1:5f537df9dca8 1092 {
GIPetrou 1:5f537df9dca8 1093 *dGain = packet.parameter[0];
GIPetrou 1:5f537df9dca8 1094
GIPetrou 1:5f537df9dca8 1095 #ifdef MX28_DEBUG
GIPetrou 1:5f537df9dca8 1096 pc->printf("Get D gain: 0x%02X\r\n", *dGain);
GIPetrou 1:5f537df9dca8 1097 #endif
GIPetrou 1:5f537df9dca8 1098 }
GIPetrou 1:5f537df9dca8 1099
GIPetrou 1:5f537df9dca8 1100 return status;
GIPetrou 1:5f537df9dca8 1101 }
GIPetrou 1:5f537df9dca8 1102
GIPetrou 1:5f537df9dca8 1103 uint8_t MX28::SetDGain(uint8_t servoId, uint8_t dGain, bool isRegWrite)
GIPetrou 1:5f537df9dca8 1104 {
GIPetrou 1:5f537df9dca8 1105 MX28_PROTOCOL_PACKET packet;
GIPetrou 1:5f537df9dca8 1106
GIPetrou 1:5f537df9dca8 1107 packet.servoId = servoId;
GIPetrou 1:5f537df9dca8 1108 packet.length = 4;
GIPetrou 1:5f537df9dca8 1109 packet.instructionErrorId = (isRegWrite == true) ? MX28_REG_WRITE : MX28_WRITE_DATA;
GIPetrou 1:5f537df9dca8 1110 packet.parameter[0] = MX28_D_GAIN;
GIPetrou 1:5f537df9dca8 1111 packet.parameter[1] = dGain;
GIPetrou 1:5f537df9dca8 1112
GIPetrou 1:5f537df9dca8 1113 #ifdef MX28_DEBUG
GIPetrou 1:5f537df9dca8 1114 pc->printf("Set D gain: 0x%02X\r\n", dGain);
GIPetrou 1:5f537df9dca8 1115 #endif
GIPetrou 1:5f537df9dca8 1116
GIPetrou 1:5f537df9dca8 1117 return CommunicatePacket(&packet);
GIPetrou 1:5f537df9dca8 1118 }
GIPetrou 1:5f537df9dca8 1119
GIPetrou 1:5f537df9dca8 1120 uint8_t MX28::GetGoalPosition(uint8_t servoId, uint16_t *goalPosition)
GIPetrou 1:5f537df9dca8 1121 {
GIPetrou 1:5f537df9dca8 1122 MX28_PROTOCOL_PACKET packet;
GIPetrou 1:5f537df9dca8 1123
GIPetrou 1:5f537df9dca8 1124 packet.servoId = servoId;
GIPetrou 1:5f537df9dca8 1125 packet.length = 4;
GIPetrou 1:5f537df9dca8 1126 packet.instructionErrorId = MX28_READ_DATA;
GIPetrou 1:5f537df9dca8 1127 packet.parameter[0] = MX28_GOAL_POSITION_L;
GIPetrou 1:5f537df9dca8 1128 packet.parameter[1] = 0x02;
GIPetrou 1:5f537df9dca8 1129
GIPetrou 1:5f537df9dca8 1130 uint8_t status = CommunicatePacket(&packet);
GIPetrou 1:5f537df9dca8 1131
GIPetrou 1:5f537df9dca8 1132 if(status == MX28_ERRBIT_NONE)
GIPetrou 1:5f537df9dca8 1133 {
GIPetrou 1:5f537df9dca8 1134 *goalPosition = Utilities::ConvertUInt8ArrayToUInt16(packet.parameter);
GIPetrou 1:5f537df9dca8 1135
GIPetrou 1:5f537df9dca8 1136 #ifdef MX28_DEBUG
GIPetrou 1:5f537df9dca8 1137 pc->printf("Get goal position: %hu\r\n", *goalPosition);
GIPetrou 1:5f537df9dca8 1138 #endif
GIPetrou 1:5f537df9dca8 1139 }
GIPetrou 1:5f537df9dca8 1140
GIPetrou 1:5f537df9dca8 1141 return status;
GIPetrou 1:5f537df9dca8 1142 }
GIPetrou 1:5f537df9dca8 1143
GIPetrou 1:5f537df9dca8 1144 uint8_t MX28::SetGoalPosition(uint8_t servoId, uint16_t goalPosition, bool isRegWrite)
GIPetrou 1:5f537df9dca8 1145 {
GIPetrou 1:5f537df9dca8 1146 MX28_PROTOCOL_PACKET packet;
GIPetrou 1:5f537df9dca8 1147
GIPetrou 1:5f537df9dca8 1148 packet.servoId = servoId;
GIPetrou 1:5f537df9dca8 1149 packet.length = 5;
GIPetrou 1:5f537df9dca8 1150 packet.instructionErrorId = (isRegWrite == true) ? MX28_REG_WRITE : MX28_WRITE_DATA;
GIPetrou 1:5f537df9dca8 1151 packet.parameter[0] = MX28_GOAL_POSITION_L;
GIPetrou 1:5f537df9dca8 1152 Utilities::ConvertUInt16ToUInt8Array(goalPosition, (uint8_t*)&(packet.parameter[1]));
GIPetrou 1:5f537df9dca8 1153
GIPetrou 1:5f537df9dca8 1154 #ifdef MX28_DEBUG
GIPetrou 1:5f537df9dca8 1155 pc->printf("Set goal position: %hu\r\n", goalPosition);
GIPetrou 1:5f537df9dca8 1156 #endif
GIPetrou 1:5f537df9dca8 1157
GIPetrou 1:5f537df9dca8 1158 return CommunicatePacket(&packet);
GIPetrou 1:5f537df9dca8 1159 }
GIPetrou 1:5f537df9dca8 1160
GIPetrou 1:5f537df9dca8 1161 uint8_t MX28::GetMovingSpeed(uint8_t servoId, uint16_t *movingSpeed)
GIPetrou 1:5f537df9dca8 1162 {
GIPetrou 1:5f537df9dca8 1163 MX28_PROTOCOL_PACKET packet;
GIPetrou 1:5f537df9dca8 1164
GIPetrou 1:5f537df9dca8 1165 packet.servoId = servoId;
GIPetrou 1:5f537df9dca8 1166 packet.length = 4;
GIPetrou 1:5f537df9dca8 1167 packet.instructionErrorId = MX28_READ_DATA;
GIPetrou 1:5f537df9dca8 1168 packet.parameter[0] = MX28_MOVING_SPEED_L;
GIPetrou 1:5f537df9dca8 1169 packet.parameter[1] = 0x02;
GIPetrou 1:5f537df9dca8 1170
GIPetrou 1:5f537df9dca8 1171 uint8_t status = CommunicatePacket(&packet);
GIPetrou 1:5f537df9dca8 1172
GIPetrou 1:5f537df9dca8 1173 if(status == MX28_ERRBIT_NONE)
GIPetrou 1:5f537df9dca8 1174 {
GIPetrou 1:5f537df9dca8 1175 *movingSpeed = Utilities::ConvertUInt8ArrayToUInt16(packet.parameter);
GIPetrou 1:5f537df9dca8 1176
GIPetrou 1:5f537df9dca8 1177 #ifdef MX28_DEBUG
GIPetrou 1:5f537df9dca8 1178 pc->printf("Get moving speed: %hu\r\n", *movingSpeed);
GIPetrou 1:5f537df9dca8 1179 #endif
GIPetrou 1:5f537df9dca8 1180 }
GIPetrou 1:5f537df9dca8 1181
GIPetrou 1:5f537df9dca8 1182 return status;
GIPetrou 1:5f537df9dca8 1183 }
GIPetrou 1:5f537df9dca8 1184
GIPetrou 1:5f537df9dca8 1185 uint8_t MX28::SetMovingSpeed(uint8_t servoId, uint16_t movingSpeed, bool isRegWrite)
GIPetrou 1:5f537df9dca8 1186 {
GIPetrou 1:5f537df9dca8 1187 MX28_PROTOCOL_PACKET packet;
GIPetrou 1:5f537df9dca8 1188
GIPetrou 1:5f537df9dca8 1189 packet.servoId = servoId;
GIPetrou 1:5f537df9dca8 1190 packet.length = 5;
GIPetrou 1:5f537df9dca8 1191 packet.instructionErrorId = (isRegWrite == true) ? MX28_REG_WRITE : MX28_WRITE_DATA;
GIPetrou 1:5f537df9dca8 1192 packet.parameter[0] = MX28_MOVING_SPEED_L;
GIPetrou 1:5f537df9dca8 1193 Utilities::ConvertUInt16ToUInt8Array(movingSpeed, (uint8_t*)&(packet.parameter[1]));
GIPetrou 1:5f537df9dca8 1194
GIPetrou 1:5f537df9dca8 1195 #ifdef MX28_DEBUG
GIPetrou 1:5f537df9dca8 1196 pc->printf("Set moving speed: %hu\r\n", movingSpeed);
GIPetrou 1:5f537df9dca8 1197 #endif
GIPetrou 1:5f537df9dca8 1198
GIPetrou 1:5f537df9dca8 1199 return CommunicatePacket(&packet);
GIPetrou 1:5f537df9dca8 1200 }
GIPetrou 1:5f537df9dca8 1201
GIPetrou 1:5f537df9dca8 1202 uint8_t MX28::GetTorqueLimit(uint8_t servoId, uint16_t *torqueLimit)
GIPetrou 1:5f537df9dca8 1203 {
GIPetrou 1:5f537df9dca8 1204 MX28_PROTOCOL_PACKET packet;
GIPetrou 1:5f537df9dca8 1205
GIPetrou 1:5f537df9dca8 1206 packet.servoId = servoId;
GIPetrou 1:5f537df9dca8 1207 packet.length = 4;
GIPetrou 1:5f537df9dca8 1208 packet.instructionErrorId = MX28_READ_DATA;
GIPetrou 1:5f537df9dca8 1209 packet.parameter[0] = MX28_TORQUE_LIMIT_L;
GIPetrou 1:5f537df9dca8 1210 packet.parameter[1] = 0x02;
GIPetrou 1:5f537df9dca8 1211
GIPetrou 1:5f537df9dca8 1212 uint8_t status = CommunicatePacket(&packet);
GIPetrou 1:5f537df9dca8 1213
GIPetrou 1:5f537df9dca8 1214 if(status == MX28_ERRBIT_NONE)
GIPetrou 1:5f537df9dca8 1215 {
GIPetrou 1:5f537df9dca8 1216 *torqueLimit = Utilities::ConvertUInt8ArrayToUInt16(packet.parameter);
GIPetrou 1:5f537df9dca8 1217
GIPetrou 1:5f537df9dca8 1218 #ifdef MX28_DEBUG
GIPetrou 1:5f537df9dca8 1219 pc->printf("Torque limit: %hu\r\n", *torqueLimit);
GIPetrou 1:5f537df9dca8 1220 #endif
GIPetrou 1:5f537df9dca8 1221 }
GIPetrou 1:5f537df9dca8 1222
GIPetrou 1:5f537df9dca8 1223 return status;
GIPetrou 1:5f537df9dca8 1224 }
GIPetrou 1:5f537df9dca8 1225
GIPetrou 1:5f537df9dca8 1226 uint8_t MX28::SetTorqueLimit(uint8_t servoId, uint16_t torqueLimit, bool isRegWrite)
GIPetrou 1:5f537df9dca8 1227 {
GIPetrou 1:5f537df9dca8 1228 MX28_PROTOCOL_PACKET packet;
GIPetrou 1:5f537df9dca8 1229
GIPetrou 1:5f537df9dca8 1230 packet.servoId = servoId;
GIPetrou 1:5f537df9dca8 1231 packet.length = 5;
GIPetrou 1:5f537df9dca8 1232 packet.instructionErrorId = (isRegWrite == true) ? MX28_REG_WRITE : MX28_WRITE_DATA;
GIPetrou 1:5f537df9dca8 1233 packet.parameter[0] = MX28_TORQUE_LIMIT_L;
GIPetrou 1:5f537df9dca8 1234 Utilities::ConvertUInt16ToUInt8Array(torqueLimit, (uint8_t*)&(packet.parameter[1]));
GIPetrou 1:5f537df9dca8 1235
GIPetrou 1:5f537df9dca8 1236 #ifdef MX28_DEBUG
GIPetrou 1:5f537df9dca8 1237 pc->printf("Set torque limit: %hu\r\n", torqueLimit);
GIPetrou 1:5f537df9dca8 1238 #endif
GIPetrou 1:5f537df9dca8 1239
GIPetrou 1:5f537df9dca8 1240 return CommunicatePacket(&packet);
GIPetrou 1:5f537df9dca8 1241 }
GIPetrou 1:5f537df9dca8 1242
GIPetrou 1:5f537df9dca8 1243 uint8_t MX28::GetPresentPosition(uint8_t servoId, uint16_t *presentPosition)
GIPetrou 1:5f537df9dca8 1244 {
GIPetrou 1:5f537df9dca8 1245 MX28_PROTOCOL_PACKET packet;
GIPetrou 1:5f537df9dca8 1246
GIPetrou 1:5f537df9dca8 1247 packet.servoId = servoId;
GIPetrou 1:5f537df9dca8 1248 packet.length = 4;
GIPetrou 1:5f537df9dca8 1249 packet.instructionErrorId = MX28_READ_DATA;
GIPetrou 1:5f537df9dca8 1250 packet.parameter[0] = MX28_PRESENT_POSITION_L;
GIPetrou 1:5f537df9dca8 1251 packet.parameter[1] = 0x02;
GIPetrou 1:5f537df9dca8 1252
GIPetrou 1:5f537df9dca8 1253 uint8_t status = CommunicatePacket(&packet);
GIPetrou 1:5f537df9dca8 1254
GIPetrou 1:5f537df9dca8 1255 if(status == MX28_ERRBIT_NONE)
GIPetrou 1:5f537df9dca8 1256 {
GIPetrou 1:5f537df9dca8 1257 *presentPosition = Utilities::ConvertUInt8ArrayToUInt16(packet.parameter);
GIPetrou 1:5f537df9dca8 1258
GIPetrou 1:5f537df9dca8 1259 #ifdef MX28_DEBUG
GIPetrou 1:5f537df9dca8 1260 pc->printf("Get present position: %hu\r\n", *presentPosition);
GIPetrou 1:5f537df9dca8 1261 #endif
GIPetrou 1:5f537df9dca8 1262 }
GIPetrou 1:5f537df9dca8 1263
GIPetrou 1:5f537df9dca8 1264 return status;
GIPetrou 1:5f537df9dca8 1265 }
GIPetrou 1:5f537df9dca8 1266
GIPetrou 1:5f537df9dca8 1267 uint8_t MX28::GetPresentSpeed(uint8_t servoId, uint16_t *presentSpeed)
GIPetrou 1:5f537df9dca8 1268 {
GIPetrou 1:5f537df9dca8 1269 MX28_PROTOCOL_PACKET packet;
GIPetrou 1:5f537df9dca8 1270
GIPetrou 1:5f537df9dca8 1271 packet.servoId = servoId;
GIPetrou 1:5f537df9dca8 1272 packet.length = 4;
GIPetrou 1:5f537df9dca8 1273 packet.instructionErrorId = MX28_READ_DATA;
GIPetrou 1:5f537df9dca8 1274 packet.parameter[0] = MX28_PRESENT_SPEED_L;
GIPetrou 1:5f537df9dca8 1275 packet.parameter[1] = 0x02;
GIPetrou 1:5f537df9dca8 1276
GIPetrou 1:5f537df9dca8 1277 uint8_t status = CommunicatePacket(&packet);
GIPetrou 1:5f537df9dca8 1278
GIPetrou 1:5f537df9dca8 1279 if(status == MX28_ERRBIT_NONE)
GIPetrou 1:5f537df9dca8 1280 {
GIPetrou 1:5f537df9dca8 1281 *presentSpeed = Utilities::ConvertUInt8ArrayToUInt16(packet.parameter);
GIPetrou 1:5f537df9dca8 1282
GIPetrou 1:5f537df9dca8 1283 #ifdef MX28_DEBUG
GIPetrou 1:5f537df9dca8 1284 pc->printf("Get present speed: %hu\r\n", *presentSpeed);
GIPetrou 1:5f537df9dca8 1285 #endif
GIPetrou 1:5f537df9dca8 1286 }
GIPetrou 1:5f537df9dca8 1287
GIPetrou 1:5f537df9dca8 1288 return status;
GIPetrou 1:5f537df9dca8 1289 }
GIPetrou 1:5f537df9dca8 1290
GIPetrou 1:5f537df9dca8 1291 uint8_t MX28::GetPresentLoad(uint8_t servoId, uint16_t *presentLoad)
GIPetrou 1:5f537df9dca8 1292 {
GIPetrou 1:5f537df9dca8 1293 MX28_PROTOCOL_PACKET packet;
GIPetrou 1:5f537df9dca8 1294
GIPetrou 1:5f537df9dca8 1295 packet.servoId = servoId;
GIPetrou 1:5f537df9dca8 1296 packet.length = 4;
GIPetrou 1:5f537df9dca8 1297 packet.instructionErrorId = MX28_READ_DATA;
GIPetrou 1:5f537df9dca8 1298 packet.parameter[0] = MX28_PRESENT_LOAD_L;
GIPetrou 1:5f537df9dca8 1299 packet.parameter[1] = 0x02;
GIPetrou 1:5f537df9dca8 1300
GIPetrou 1:5f537df9dca8 1301 uint8_t status = CommunicatePacket(&packet);
GIPetrou 1:5f537df9dca8 1302
GIPetrou 1:5f537df9dca8 1303 if(status == MX28_ERRBIT_NONE)
GIPetrou 1:5f537df9dca8 1304 {
GIPetrou 1:5f537df9dca8 1305 *presentLoad = Utilities::ConvertUInt8ArrayToUInt16(packet.parameter);
GIPetrou 1:5f537df9dca8 1306
GIPetrou 1:5f537df9dca8 1307 #ifdef MX28_DEBUG
GIPetrou 1:5f537df9dca8 1308 pc->printf("Get present load: %hu\r\n", *presentLoad);
GIPetrou 1:5f537df9dca8 1309 #endif
GIPetrou 1:5f537df9dca8 1310 }
GIPetrou 1:5f537df9dca8 1311
GIPetrou 1:5f537df9dca8 1312 return status;
GIPetrou 1:5f537df9dca8 1313 }
GIPetrou 1:5f537df9dca8 1314
GIPetrou 1:5f537df9dca8 1315 uint8_t MX28::GetPresentVoltage(uint8_t servoId, uint8_t *presentVoltage)
GIPetrou 1:5f537df9dca8 1316 {
GIPetrou 1:5f537df9dca8 1317 MX28_PROTOCOL_PACKET packet;
GIPetrou 1:5f537df9dca8 1318
GIPetrou 1:5f537df9dca8 1319 packet.servoId = servoId;
GIPetrou 1:5f537df9dca8 1320 packet.length = 4;
GIPetrou 1:5f537df9dca8 1321 packet.instructionErrorId = MX28_READ_DATA;
GIPetrou 1:5f537df9dca8 1322 packet.parameter[0] = MX28_PRESENT_VOLTAGE;
GIPetrou 1:5f537df9dca8 1323 packet.parameter[1] = 0x01;
GIPetrou 1:5f537df9dca8 1324
GIPetrou 1:5f537df9dca8 1325 uint8_t status = CommunicatePacket(&packet);
GIPetrou 1:5f537df9dca8 1326
GIPetrou 1:5f537df9dca8 1327 if(status == MX28_ERRBIT_NONE)
GIPetrou 1:5f537df9dca8 1328 {
GIPetrou 1:5f537df9dca8 1329 *presentVoltage = packet.parameter[0];
GIPetrou 1:5f537df9dca8 1330
GIPetrou 1:5f537df9dca8 1331 #ifdef MX28_DEBUG
GIPetrou 1:5f537df9dca8 1332 pc->printf("Get present voltage: 0x%02X\r\n", *presentVoltage);
GIPetrou 1:5f537df9dca8 1333 #endif
GIPetrou 1:5f537df9dca8 1334 }
GIPetrou 1:5f537df9dca8 1335
GIPetrou 1:5f537df9dca8 1336 return status;
GIPetrou 1:5f537df9dca8 1337 }
GIPetrou 1:5f537df9dca8 1338
GIPetrou 1:5f537df9dca8 1339 uint8_t MX28::GetPresentTemperature(uint8_t servoId, uint8_t *presentTemperature)
GIPetrou 1:5f537df9dca8 1340 {
GIPetrou 1:5f537df9dca8 1341 MX28_PROTOCOL_PACKET packet;
GIPetrou 1:5f537df9dca8 1342
GIPetrou 1:5f537df9dca8 1343 packet.servoId = servoId;
GIPetrou 1:5f537df9dca8 1344 packet.length = 4;
GIPetrou 1:5f537df9dca8 1345 packet.instructionErrorId = MX28_READ_DATA;
GIPetrou 1:5f537df9dca8 1346 packet.parameter[0] = MX28_PRESENT_TEMPERATURE;
GIPetrou 1:5f537df9dca8 1347 packet.parameter[1] = 0x01;
GIPetrou 1:5f537df9dca8 1348
GIPetrou 1:5f537df9dca8 1349 uint8_t status = CommunicatePacket(&packet);
GIPetrou 1:5f537df9dca8 1350
GIPetrou 1:5f537df9dca8 1351 if(status == MX28_ERRBIT_NONE)
GIPetrou 1:5f537df9dca8 1352 {
GIPetrou 1:5f537df9dca8 1353 *presentTemperature = packet.parameter[0];
GIPetrou 1:5f537df9dca8 1354
GIPetrou 1:5f537df9dca8 1355 #ifdef MX28_DEBUG
GIPetrou 1:5f537df9dca8 1356 pc->printf("Get present temperature: 0x%02X\r\n", *presentTemperature);
GIPetrou 1:5f537df9dca8 1357 #endif
GIPetrou 1:5f537df9dca8 1358 }
GIPetrou 1:5f537df9dca8 1359
GIPetrou 1:5f537df9dca8 1360 return status;
GIPetrou 1:5f537df9dca8 1361 }
GIPetrou 1:5f537df9dca8 1362
GIPetrou 1:5f537df9dca8 1363 uint8_t MX28::GetIsRegistered(uint8_t servoId, uint8_t *registered)
GIPetrou 1:5f537df9dca8 1364 {
GIPetrou 1:5f537df9dca8 1365 MX28_PROTOCOL_PACKET packet;
GIPetrou 1:5f537df9dca8 1366
GIPetrou 1:5f537df9dca8 1367 packet.servoId = servoId;
GIPetrou 1:5f537df9dca8 1368 packet.length = 4;
GIPetrou 1:5f537df9dca8 1369 packet.instructionErrorId = MX28_READ_DATA;
GIPetrou 1:5f537df9dca8 1370 packet.parameter[0] = MX28_PRESENT_TEMPERATURE;
GIPetrou 1:5f537df9dca8 1371 packet.parameter[1] = 0x01;
GIPetrou 1:5f537df9dca8 1372
GIPetrou 1:5f537df9dca8 1373 uint8_t status = CommunicatePacket(&packet);
GIPetrou 1:5f537df9dca8 1374
GIPetrou 1:5f537df9dca8 1375 if(status == MX28_ERRBIT_NONE)
GIPetrou 1:5f537df9dca8 1376 {
GIPetrou 1:5f537df9dca8 1377 *registered = packet.parameter[0];
GIPetrou 1:5f537df9dca8 1378
GIPetrou 1:5f537df9dca8 1379 #ifdef MX28_DEBUG
GIPetrou 1:5f537df9dca8 1380 pc->printf("Get is registered: 0x%02X\r\n", *registered);
GIPetrou 1:5f537df9dca8 1381 #endif
GIPetrou 1:5f537df9dca8 1382 }
GIPetrou 1:5f537df9dca8 1383
GIPetrou 1:5f537df9dca8 1384 return status;
GIPetrou 1:5f537df9dca8 1385 }
GIPetrou 1:5f537df9dca8 1386
GIPetrou 1:5f537df9dca8 1387 uint8_t MX28::GetIsMoving(uint8_t servoId, uint8_t *moving)
GIPetrou 1:5f537df9dca8 1388 {
GIPetrou 1:5f537df9dca8 1389 MX28_PROTOCOL_PACKET packet;
GIPetrou 1:5f537df9dca8 1390
GIPetrou 1:5f537df9dca8 1391 packet.servoId = servoId;
GIPetrou 1:5f537df9dca8 1392 packet.length = 4;
GIPetrou 1:5f537df9dca8 1393 packet.instructionErrorId = MX28_READ_DATA;
GIPetrou 1:5f537df9dca8 1394 packet.parameter[0] = MX28_PRESENT_TEMPERATURE;
GIPetrou 1:5f537df9dca8 1395 packet.parameter[1] = 0x01;
GIPetrou 1:5f537df9dca8 1396
GIPetrou 1:5f537df9dca8 1397 uint8_t status = CommunicatePacket(&packet);
GIPetrou 1:5f537df9dca8 1398
GIPetrou 1:5f537df9dca8 1399 if(status == MX28_ERRBIT_NONE)
GIPetrou 1:5f537df9dca8 1400 {
GIPetrou 1:5f537df9dca8 1401 *moving = packet.parameter[0];
GIPetrou 1:5f537df9dca8 1402
GIPetrou 1:5f537df9dca8 1403 #ifdef MX28_DEBUG
GIPetrou 1:5f537df9dca8 1404 pc->printf("Get is moving: 0x%02X\r\n", *moving);
GIPetrou 1:5f537df9dca8 1405 #endif
GIPetrou 1:5f537df9dca8 1406 }
GIPetrou 1:5f537df9dca8 1407
GIPetrou 1:5f537df9dca8 1408 return status;
GIPetrou 1:5f537df9dca8 1409 }
GIPetrou 1:5f537df9dca8 1410
GIPetrou 1:5f537df9dca8 1411 uint8_t MX28::GetIsLocked(uint8_t servoId, uint8_t *isLocked)
GIPetrou 1:5f537df9dca8 1412 {
GIPetrou 1:5f537df9dca8 1413 MX28_PROTOCOL_PACKET packet;
GIPetrou 1:5f537df9dca8 1414
GIPetrou 1:5f537df9dca8 1415 packet.servoId = servoId;
GIPetrou 1:5f537df9dca8 1416 packet.length = 4;
GIPetrou 1:5f537df9dca8 1417 packet.instructionErrorId = MX28_READ_DATA;
GIPetrou 1:5f537df9dca8 1418 packet.parameter[0] = MX28_LOCK;
GIPetrou 1:5f537df9dca8 1419 packet.parameter[1] = 0x01;
GIPetrou 1:5f537df9dca8 1420
GIPetrou 1:5f537df9dca8 1421 uint8_t status = CommunicatePacket(&packet);
GIPetrou 1:5f537df9dca8 1422
GIPetrou 1:5f537df9dca8 1423 if(status == MX28_ERRBIT_NONE)
GIPetrou 1:5f537df9dca8 1424 {
GIPetrou 1:5f537df9dca8 1425 *isLocked = packet.parameter[0];
GIPetrou 1:5f537df9dca8 1426
GIPetrou 1:5f537df9dca8 1427 #ifdef MX28_DEBUG
GIPetrou 1:5f537df9dca8 1428 pc->printf("Get is locked: 0x%02X\r\n", *isLocked);
GIPetrou 1:5f537df9dca8 1429 #endif
GIPetrou 1:5f537df9dca8 1430 }
GIPetrou 1:5f537df9dca8 1431
GIPetrou 1:5f537df9dca8 1432 return status;
GIPetrou 1:5f537df9dca8 1433 }
GIPetrou 1:5f537df9dca8 1434
GIPetrou 1:5f537df9dca8 1435 uint8_t MX28::SetIsLocked(uint8_t servoId, uint8_t isLocked, bool isRegWrite)
GIPetrou 1:5f537df9dca8 1436 {
GIPetrou 1:5f537df9dca8 1437 MX28_PROTOCOL_PACKET packet;
GIPetrou 1:5f537df9dca8 1438
GIPetrou 1:5f537df9dca8 1439 packet.servoId = servoId;
GIPetrou 1:5f537df9dca8 1440 packet.length = 4;
GIPetrou 1:5f537df9dca8 1441 packet.instructionErrorId = (isRegWrite == true) ? MX28_REG_WRITE : MX28_WRITE_DATA;
GIPetrou 1:5f537df9dca8 1442 packet.parameter[0] = MX28_LOCK;
GIPetrou 1:5f537df9dca8 1443 packet.parameter[1] = isLocked;
GIPetrou 1:5f537df9dca8 1444
GIPetrou 1:5f537df9dca8 1445 #ifdef MX28_DEBUG
GIPetrou 1:5f537df9dca8 1446 pc->printf("Set is locked: 0x%02X\r\n", isLocked);
GIPetrou 1:5f537df9dca8 1447 #endif
GIPetrou 1:5f537df9dca8 1448
GIPetrou 1:5f537df9dca8 1449 return CommunicatePacket(&packet);
GIPetrou 1:5f537df9dca8 1450 }
GIPetrou 1:5f537df9dca8 1451
GIPetrou 1:5f537df9dca8 1452 uint8_t MX28::GetPunch(uint8_t servoId, uint16_t *punch)
GIPetrou 1:5f537df9dca8 1453 {
GIPetrou 1:5f537df9dca8 1454 MX28_PROTOCOL_PACKET packet;
GIPetrou 1:5f537df9dca8 1455
GIPetrou 1:5f537df9dca8 1456 packet.servoId = servoId;
GIPetrou 1:5f537df9dca8 1457 packet.length = 4;
GIPetrou 1:5f537df9dca8 1458 packet.instructionErrorId = MX28_READ_DATA;
GIPetrou 1:5f537df9dca8 1459 packet.parameter[0] = MX28_PUNCH_L;
GIPetrou 1:5f537df9dca8 1460 packet.parameter[1] = 0x02;
GIPetrou 1:5f537df9dca8 1461
GIPetrou 1:5f537df9dca8 1462 uint8_t status = CommunicatePacket(&packet);
GIPetrou 1:5f537df9dca8 1463
GIPetrou 1:5f537df9dca8 1464 if(status == MX28_ERRBIT_NONE)
GIPetrou 1:5f537df9dca8 1465 {
GIPetrou 1:5f537df9dca8 1466 *punch = Utilities::ConvertUInt8ArrayToUInt16(packet.parameter);
GIPetrou 1:5f537df9dca8 1467
GIPetrou 1:5f537df9dca8 1468 #ifdef MX28_DEBUG
GIPetrou 1:5f537df9dca8 1469 pc->printf("Get punch: %hu\r\n", *punch);
GIPetrou 1:5f537df9dca8 1470 #endif
GIPetrou 1:5f537df9dca8 1471 }
GIPetrou 1:5f537df9dca8 1472
GIPetrou 1:5f537df9dca8 1473 return status;
GIPetrou 1:5f537df9dca8 1474 }
GIPetrou 1:5f537df9dca8 1475
GIPetrou 1:5f537df9dca8 1476 uint8_t MX28::SetPunch(uint8_t servoId, uint16_t punch, bool isRegWrite)
GIPetrou 1:5f537df9dca8 1477 {
GIPetrou 1:5f537df9dca8 1478 MX28_PROTOCOL_PACKET packet;
GIPetrou 1:5f537df9dca8 1479
GIPetrou 1:5f537df9dca8 1480 packet.servoId = servoId;
GIPetrou 1:5f537df9dca8 1481 packet.length = 5;
GIPetrou 1:5f537df9dca8 1482 packet.instructionErrorId = (isRegWrite == true) ? MX28_REG_WRITE : MX28_WRITE_DATA;
GIPetrou 1:5f537df9dca8 1483 packet.parameter[0] = MX28_PUNCH_L;
GIPetrou 1:5f537df9dca8 1484 Utilities::ConvertUInt16ToUInt8Array(punch, (uint8_t*)&(packet.parameter[1]));
GIPetrou 1:5f537df9dca8 1485
GIPetrou 1:5f537df9dca8 1486 #ifdef MX28_DEBUG
GIPetrou 1:5f537df9dca8 1487 pc->printf("Set punch: %hu\r\n", punch);
GIPetrou 1:5f537df9dca8 1488 #endif
GIPetrou 1:5f537df9dca8 1489
GIPetrou 1:5f537df9dca8 1490 return CommunicatePacket(&packet);
GIPetrou 1:5f537df9dca8 1491 }
GIPetrou 1:5f537df9dca8 1492
GIPetrou 1:5f537df9dca8 1493 uint8_t MX28::Ping(uint8_t servoId)
GIPetrou 1:5f537df9dca8 1494 {
GIPetrou 1:5f537df9dca8 1495 MX28_PROTOCOL_PACKET packet;
GIPetrou 1:5f537df9dca8 1496
GIPetrou 1:5f537df9dca8 1497 packet.servoId = servoId;
GIPetrou 1:5f537df9dca8 1498 packet.length = 2;
GIPetrou 1:5f537df9dca8 1499 packet.instructionErrorId = MX28_PING;
GIPetrou 1:5f537df9dca8 1500
GIPetrou 1:5f537df9dca8 1501 #ifdef MX28_DEBUG
GIPetrou 1:5f537df9dca8 1502 pc->printf("Ping\r\n");
GIPetrou 1:5f537df9dca8 1503 #endif
GIPetrou 1:5f537df9dca8 1504
GIPetrou 1:5f537df9dca8 1505 return CommunicatePacket(&packet);
GIPetrou 1:5f537df9dca8 1506 }
GIPetrou 1:5f537df9dca8 1507
GIPetrou 1:5f537df9dca8 1508 uint8_t MX28::Reset(uint8_t servoId)
GIPetrou 1:5f537df9dca8 1509 {
GIPetrou 1:5f537df9dca8 1510 MX28_PROTOCOL_PACKET packet;
GIPetrou 1:5f537df9dca8 1511
GIPetrou 1:5f537df9dca8 1512 packet.servoId = servoId;
GIPetrou 1:5f537df9dca8 1513 packet.length = 2;
GIPetrou 1:5f537df9dca8 1514 packet.instructionErrorId = MX28_RESET;
GIPetrou 1:5f537df9dca8 1515
GIPetrou 1:5f537df9dca8 1516 #ifdef MX28_DEBUG
GIPetrou 1:5f537df9dca8 1517 pc->printf("Reset\r\n");
GIPetrou 1:5f537df9dca8 1518 #endif
GIPetrou 1:5f537df9dca8 1519
GIPetrou 1:5f537df9dca8 1520 return CommunicatePacket(&packet);
GIPetrou 1:5f537df9dca8 1521 }
GIPetrou 1:5f537df9dca8 1522
GIPetrou 1:5f537df9dca8 1523 uint8_t MX28::Action(uint8_t servoId)
GIPetrou 1:5f537df9dca8 1524 {
GIPetrou 1:5f537df9dca8 1525 MX28_PROTOCOL_PACKET packet;
GIPetrou 1:5f537df9dca8 1526
GIPetrou 1:5f537df9dca8 1527 packet.servoId = servoId;
GIPetrou 1:5f537df9dca8 1528 packet.length = 2;
GIPetrou 1:5f537df9dca8 1529 packet.instructionErrorId = MX28_ACTION;
GIPetrou 1:5f537df9dca8 1530
GIPetrou 1:5f537df9dca8 1531 #ifdef MX28_DEBUG
GIPetrou 1:5f537df9dca8 1532 pc->printf("Action\r\n");
GIPetrou 1:5f537df9dca8 1533 #endif
GIPetrou 1:5f537df9dca8 1534
GIPetrou 1:5f537df9dca8 1535 return CommunicatePacket(&packet);
GIPetrou 1:5f537df9dca8 1536 }
GIPetrou 1:5f537df9dca8 1537
GIPetrou 1:5f537df9dca8 1538 MX28::MX28(PinName tx, PinName rx, int baudRate)
GIPetrou 1:5f537df9dca8 1539 {
soulx 3:7929abcdc22f 1540
soulx 3:7929abcdc22f 1541
GIPetrou 1:5f537df9dca8 1542 #ifdef MX28_DEBUG
GIPetrou 1:5f537df9dca8 1543 pc = new Serial(USBTX, USBRX);
GIPetrou 1:5f537df9dca8 1544 pc->baud(115200);
GIPetrou 1:5f537df9dca8 1545 pc->printf("\033[2J");
GIPetrou 1:5f537df9dca8 1546 #endif
soulx 3:7929abcdc22f 1547 //pc->printf("baud1= %d\r\n",baudRate);
soulx 3:7929abcdc22f 1548 //servoSerialHalfDuplex = new SerialHalfDuplex(tx, rx);
soulx 3:7929abcdc22f 1549 servoSerialHalfDuplex = new iSerial(tx, rx,NULL,1000,100);
soulx 3:7929abcdc22f 1550 //pc->printf("baud2= %d\r\n",baudRate);
GIPetrou 1:5f537df9dca8 1551 servoSerialHalfDuplex->baud(baudRate);
soulx 3:7929abcdc22f 1552 //servoSerialHalfDuplex->baud(1000000);
soulx 3:7929abcdc22f 1553 //servoSerialHalfDuplex->printf("a");
GIPetrou 1:5f537df9dca8 1554 }
GIPetrou 1:5f537df9dca8 1555
GIPetrou 1:5f537df9dca8 1556 MX28::~MX28()
GIPetrou 1:5f537df9dca8 1557 {
GIPetrou 1:5f537df9dca8 1558 #ifdef MX28_DEBUG
GIPetrou 1:5f537df9dca8 1559 if(pc != NULL)
GIPetrou 1:5f537df9dca8 1560 delete pc;
GIPetrou 1:5f537df9dca8 1561 #endif
GIPetrou 1:5f537df9dca8 1562
GIPetrou 1:5f537df9dca8 1563 if(servoSerialHalfDuplex != NULL)
GIPetrou 1:5f537df9dca8 1564 delete servoSerialHalfDuplex;
GIPetrou 1:5f537df9dca8 1565 }