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@2:85216442d3ef, 2013-04-03 (annotated)

Committer:
GIPetrou
Date:
Wed Apr 03 16:44:50 2013 +0000
Revision:
2:85216442d3ef
Parent:
1:5f537df9dca8 
Updated code to work with latest mbed version

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