Georgios Petrou / MX28

A library to control MX28 servos. It could also be used with the rest of the MX series servos.

Dependents:   IDcheck

Fork of MX28 by Georgios Petrou

This library is based on Robotis documentation regarding the dynamixel and MX28 protocols

It is part of a bigger project involving seven mbeds to control a hexapod robot.

I have not tried to control other MX series servos, but it should be possible to use this library with minor modifications.

MX28.cpp@0:ea5b951002cf, 2012-09-09 (annotated)

Committer:
GIPetrou
Date:
Sun Sep 09 22:09:00 2012 +0000
Revision:
0:ea5b951002cf
Child:
1:5f537df9dca8
First version of library

Who changed what in which revision?

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