Georgios Petrou
A library to control MX28 servos. It could also be used with the rest of the MX series servos.
Fork of
MX28
by 
Georgios Petrou
This library is based on Robotis documentation regarding the dynamixel and MX28 protocols
- http://robosavvy.com/docs/Bioloid/AX-12(english).pdf
- http://support.robotis.com/en/product/dynamixel/mx_series/mx-28.htm
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.
Revision 0:ea5b951002cf, committed 2012-09-09
- Comitter:
- GIPetrou
- Date:
- Sun Sep 09 22:09:00 2012 +0000
- Child:
- 1:5f537df9dca8
- Commit message:
- First version of library
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/MX28.cpp Sun Sep 09 22:09:00 2012 +0000
@@ -0,0 +1,1526 @@
+/* Copyright (c) 2012 Georgios Petrou, MIT License
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy of this software
+ * and associated documentation files (the "Software"), to deal in the Software without restriction,
+ * including without limitation the rights to use, copy, modify, merge, publish, distribute,
+ * sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all copies or
+ * substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
+ * BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
+ * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
+
+ #include "MX28.h"
+
+uint8_t MX28::CommunicatePacket(MX28_PROTOCOL_PACKET *packet)
+{
+ uint8_t currentParameter = 0;
+ bool isWholePacket = false;
+ uint8_t encoderState = WAIT_ON_HEADER_0;
+
+ packet->checkSum = Utilities::GetCheckSum((uint8_t*)&(packet->servoId), packet->length + 1);
+
+ Timer timer;
+ timer.start();
+
+ while((timer.read_ms() < MX28_PROTOCOL_COMMAND_RESPONSE_TIMEOUT_MS) && (!isWholePacket))
+ {
+ if(servoSerialHalfDuplex->writeable())
+ {
+ switch(encoderState)
+ {
+ case WAIT_ON_HEADER_0:
+ {
+ #ifdef MX28_DEBUG
+ pc->printf("Write: 0x%02X ", MX28_PROTOCOL_HEADER_0);
+ #endif
+
+ servoSerialHalfDuplex->putc(MX28_PROTOCOL_HEADER_0);
+
+ encoderState = WAIT_ON_HEADER_1;
+
+ break;
+ }
+ case WAIT_ON_HEADER_1:
+ {
+ #ifdef MX28_DEBUG
+ pc->printf("0x%02X ", MX28_PROTOCOL_HEADER_1);
+ #endif
+
+ servoSerialHalfDuplex->putc(MX28_PROTOCOL_HEADER_1);
+
+ encoderState = WAIT_ON_ADDRESSED_NODE_ID;
+
+ break;
+ }
+ case WAIT_ON_ADDRESSED_NODE_ID:
+ {
+ #ifdef MX28_DEBUG
+ pc->printf("0x%02X ", packet->servoId);
+ #endif
+
+ servoSerialHalfDuplex->putc(packet->servoId);
+
+ encoderState = WAIT_ON_LENGTH;
+
+ break;
+ }
+ case WAIT_ON_LENGTH:
+ {
+ #ifdef MX28_DEBUG
+ pc->printf("0x%02X ", packet->length);
+ #endif
+
+ servoSerialHalfDuplex->putc(packet->length);
+
+ encoderState = WAIT_ON_INSTRUCTION_ERROR_ID;
+
+ break;
+ }
+ case WAIT_ON_INSTRUCTION_ERROR_ID:
+ {
+ #ifdef MX28_DEBUG
+ pc->printf("0x%02X ", packet->instructionErrorId);
+ #endif
+
+ servoSerialHalfDuplex->putc(packet->instructionErrorId);
+
+ if(packet->length > 2)
+ encoderState = WAIT_ON_PARAMETER;
+ else
+ encoderState = WAIT_ON_CHECK_SUM;
+
+ break;
+ }
+ case WAIT_ON_PARAMETER:
+ {
+ #ifdef MX28_DEBUG
+ pc->printf("0x%02X ", packet->parameter[currentParameter]);
+ #endif
+
+ servoSerialHalfDuplex->putc(packet->parameter[currentParameter]);
+
+ if(++currentParameter == packet->length - 2)
+ encoderState = WAIT_ON_CHECK_SUM;
+
+ break;
+ }
+ case WAIT_ON_CHECK_SUM:
+ {
+ #ifdef MX28_DEBUG
+ pc->printf("0x%02X\r\n", packet->checkSum);
+ #endif
+
+ servoSerialHalfDuplex->putc(packet->checkSum);
+
+ encoderState = WAIT_ON_HEADER_0;
+ isWholePacket = true;
+
+ break;
+ }
+ }
+ }
+ }
+
+ #ifdef MX28_DEBUG
+ pc->printf("Timer: %d ms\r\n", timer.read_ms());
+ #endif
+
+ timer.stop();
+
+ if(!isWholePacket)
+ {
+ #ifdef MX28_DEBUG
+ pc->printf("Error: Write response timeout.\r\n");
+ #endif
+
+ return MX28_ERRBIT_WRITE_TIMEOUT;
+ }
+
+ if( packet->servoId == MX28_PROTOCOL_BROADCAST_ID ||
+ packet->instructionErrorId == MX28_ACTION ||
+ packet->instructionErrorId == MX28_SYNC_WRITE)
+
+ return MX28_ERRBIT_NONE;
+
+ currentParameter = 0;
+ isWholePacket = false;
+ uint8_t decoderState = WAIT_ON_HEADER_0;
+
+ timer.reset();
+ timer.start();
+
+ while((timer.read_ms() < MX28_PROTOCOL_COMMAND_RESPONSE_TIMEOUT_MS) && (!isWholePacket))
+ {
+ if(servoSerialHalfDuplex->readable())
+ {
+ switch(decoderState)
+ {
+ case WAIT_ON_HEADER_0:
+ {
+ uint8_t mx28ProtocolHeader0 = servoSerialHalfDuplex->getc();
+
+ #ifdef MX28_DEBUG
+ pc->printf("Read: 0x%02X ", mx28ProtocolHeader0);
+ #endif
+
+ decoderState = WAIT_ON_HEADER_1;
+
+ break;
+ }
+ case WAIT_ON_HEADER_1:
+ {
+ uint8_t mx28ProtocolHeader1 = servoSerialHalfDuplex->getc();
+
+ #ifdef MX28_DEBUG
+ pc->printf("0x%02X ", mx28ProtocolHeader1);
+ #endif
+
+ decoderState = WAIT_ON_ADDRESSED_NODE_ID;
+
+ break;
+ }
+ case WAIT_ON_ADDRESSED_NODE_ID:
+ {
+ packet->servoId = servoSerialHalfDuplex->getc();
+
+ #ifdef MX28_DEBUG
+ pc->printf("0x%02X ", packet->servoId);
+ #endif
+
+ decoderState = WAIT_ON_LENGTH;
+
+ break;
+ }
+ case WAIT_ON_LENGTH:
+ {
+ packet->length = servoSerialHalfDuplex->getc();
+
+ #ifdef MX28_DEBUG
+ pc->printf("0x%02X ", packet->length);
+ #endif
+
+ decoderState = WAIT_ON_INSTRUCTION_ERROR_ID;
+
+ break;
+ }
+ case WAIT_ON_INSTRUCTION_ERROR_ID:
+ {
+ packet->instructionErrorId = servoSerialHalfDuplex->getc();
+
+ #ifdef MX28_DEBUG
+ pc->printf("0x%02X ", packet->instructionErrorId);
+ #endif
+
+ if(packet->length > 2)
+ decoderState = WAIT_ON_PARAMETER;
+ else
+ decoderState = WAIT_ON_CHECK_SUM;
+
+ break;
+ }
+ case WAIT_ON_PARAMETER:
+ {
+ uint8_t parameter = servoSerialHalfDuplex->getc();
+ packet->parameter[currentParameter] = parameter;
+
+ #ifdef MX28_DEBUG
+ pc->printf("0x%02X ", parameter);
+ #endif
+
+ if(++currentParameter == packet->length - 2)
+ decoderState = WAIT_ON_CHECK_SUM;
+
+ break;
+ }
+ case WAIT_ON_CHECK_SUM:
+ {
+ packet->checkSum = servoSerialHalfDuplex->getc();
+
+ #ifdef MX28_DEBUG
+ pc->printf("0x%02X\r\n", packet->checkSum);
+ #endif
+
+ decoderState = WAIT_ON_HEADER_0;
+ isWholePacket = true;
+
+ break;
+ }
+ }
+ }
+ }
+
+ #ifdef MX28_DEBUG
+ pc->printf("Timer: %d ms\r\n", timer.read_ms());
+ #endif
+
+ timer.stop();
+
+ if(!isWholePacket)
+ {
+ #ifdef MX28_DEBUG
+ pc->printf("Error: Read response timeout\r\n");
+ #endif
+
+ return MX28_ERRBIT_READ_TIMEOUT;
+ }
+
+ if(packet->instructionErrorId != MX28_ERRBIT_NONE)
+ {
+ #ifdef MX28_DEBUG
+ if(packet->instructionErrorId & MX28_ERRBIT_VOLTAGE)
+ pc->printf("Error: Input voltage error\r\n");
+ if (packet->instructionErrorId & MX28_ERRBIT_ANGLE)
+ pc->printf("Error: Angle limit error\r\n");
+ if (packet->instructionErrorId & MX28_ERRBIT_OVERHEAT)
+ pc->printf("Error: Overheat error\r\n");
+ if (packet->instructionErrorId & MX28_ERRBIT_RANGE)
+ pc->printf("Error: Out of range error\r\n");
+ if (packet->instructionErrorId & MX28_ERRBIT_CHECKSUM)
+ pc->printf("Error: Checksum error\r\n");
+ if (packet->instructionErrorId & MX28_ERRBIT_OVERLOAD)
+ pc->printf("Error: Overload error\r\n");
+ if (packet->instructionErrorId & MX28_ERRBIT_INSTRUCTION)
+ pc->printf("Error: Instruction code error\r\n");
+ #endif
+
+ return packet->instructionErrorId;
+ }
+
+ if(packet->checkSum != Utilities::GetCheckSum((uint8_t*)&(packet->servoId), packet->length + 1))
+ {
+ #ifdef MX28_DEBUG
+ pc->printf("Error: Master got wrong checksum\r\n");
+ #endif
+
+ return MX28_ERRBIT_MASTER_CHECKSUM;
+ }
+
+ return MX28_ERRBIT_NONE;
+}
+
+uint8_t MX28::GetModelNumber(uint8_t servoId, uint16_t *modelNumber)
+{
+ MX28_PROTOCOL_PACKET packet;
+
+ packet.servoId = servoId;
+ packet.length = 4;
+ packet.instructionErrorId = MX28_READ_DATA;
+ packet.parameter[0] = MX28_MODEL_NUMBER_L;
+ packet.parameter[1] = 0x02;
+
+ uint8_t status = CommunicatePacket(&packet);
+
+ if(status == MX28_ERRBIT_NONE)
+ {
+ *modelNumber = Utilities::ConvertUInt8ArrayToUInt16(packet.parameter);
+
+ #ifdef MX28_DEBUG
+ pc->printf("Get model number: %hu\r\n", *modelNumber);
+ #endif
+ }
+
+ return status;
+}
+
+uint8_t MX28::GetFirmwareVersion(uint8_t servoId, uint8_t *firmwareVersion)
+{
+ MX28_PROTOCOL_PACKET packet;
+
+ packet.servoId = servoId;
+ packet.length = 4;
+ packet.instructionErrorId = MX28_READ_DATA;
+ packet.parameter[0] = MX28_VERSION;
+ packet.parameter[1] = 0x01;
+
+ uint8_t status = CommunicatePacket(&packet);
+
+ if(status == MX28_ERRBIT_NONE)
+ {
+ *firmwareVersion = packet.parameter[0];
+
+ #ifdef MX28_DEBUG
+ pc->printf("Get firmware version: 0x%02X\r\n", *firmwareVersion);
+ #endif
+ }
+
+ return status;
+}
+
+uint8_t MX28::GetId(uint8_t servoId, uint8_t *id)
+{
+ MX28_PROTOCOL_PACKET packet;
+
+ packet.servoId = servoId;
+ packet.length = 4;
+ packet.instructionErrorId = MX28_READ_DATA;
+ packet.parameter[0] = MX28_ID;
+ packet.parameter[1] = 0x01;
+
+ uint8_t status = CommunicatePacket(&packet);
+
+ if(status == MX28_ERRBIT_NONE)
+ {
+ *id = packet.parameter[0];
+
+ #ifdef MX28_DEBUG
+ pc->printf("Get id: 0x%02X\r\n", *id);
+ #endif
+ }
+
+ return status;
+}
+
+uint8_t MX28::SetId(uint8_t servoId, uint8_t id, bool isRegWrite)
+{
+ MX28_PROTOCOL_PACKET packet;
+
+ packet.servoId = servoId;
+ packet.length = 4;
+ packet.instructionErrorId = (isRegWrite == true) ? MX28_REG_WRITE : MX28_WRITE_DATA;
+ packet.parameter[0] = MX28_ID;
+ packet.parameter[1] = id;
+
+ #ifdef MX28_DEBUG
+ pc->printf("Set id: 0x%02X\r\n", id);
+ #endif
+
+ return CommunicatePacket(&packet);
+}
+
+uint8_t MX28::GetBaudRate(uint8_t servoId, int32_t *baudRate)
+{
+ MX28_PROTOCOL_PACKET packet;
+
+ packet.servoId = servoId;
+ packet.length = 4;
+ packet.instructionErrorId = MX28_READ_DATA;
+ packet.parameter[0] = MX28_BAUD_RATE;
+ packet.parameter[1] = 1;
+
+ uint8_t status = CommunicatePacket(&packet);
+
+ if(status == MX28_ERRBIT_NONE)
+ {
+ if(packet.parameter[0] < 0xFA)
+ *baudRate = 2000000.0 / (double)(packet.parameter[0] + 1);
+ else if(packet.parameter[0] == 0xFA)
+ *baudRate = 2250000;
+ else if(packet.parameter[0] == 0xFB)
+ *baudRate = 2500000;
+ else
+ *baudRate = 3000000;
+
+ #ifdef MX28_DEBUG
+ pc->printf("Get baud rate: %d\r\n", *baudRate);
+ #endif
+ }
+
+ return status;
+}
+
+uint8_t MX28::SetBaudRate(uint8_t servoId, int32_t baudRate, bool isRegWrite)
+{
+ MX28_PROTOCOL_PACKET packet;
+
+ uint8_t divisor = 0x00;
+
+ if(baudRate < 2250000)
+ divisor = (uint8_t)(2000000.0 / (double)baudRate) - 1;
+ else if(baudRate == 2250000)
+ divisor = 0xFA;
+ else if(baudRate == 2500000)
+ divisor = 0xFB;
+ else
+ divisor = 0xFC;
+
+ packet.servoId = servoId;
+ packet.length = 4;
+ packet.instructionErrorId = (isRegWrite == true) ? MX28_REG_WRITE : MX28_WRITE_DATA;
+ packet.parameter[0] = MX28_BAUD_RATE;
+ packet.parameter[1] = divisor;
+
+ #ifdef MX28_DEBUG
+ pc->printf("Set baudrate: 0x%02X\r\n", divisor);
+ #endif
+
+ return CommunicatePacket(&packet);
+}
+
+uint8_t MX28::GetReturnDelayTime(uint8_t servoId, uint8_t *returnDelayTime)
+{
+ MX28_PROTOCOL_PACKET packet;
+
+ packet.servoId = servoId;
+ packet.length = 4;
+ packet.instructionErrorId = MX28_READ_DATA;
+ packet.parameter[0] = MX28_RETURN_DELAY_TIME;
+ packet.parameter[1] = 0x01;
+
+ uint8_t status = CommunicatePacket(&packet);
+
+ if(status == MX28_ERRBIT_NONE)
+ {
+ *returnDelayTime = packet.parameter[0];
+
+ #ifdef MX28_DEBUG
+ pc->printf("Get return delay time: 0x%02X\r\n", *returnDelayTime);
+ #endif
+ }
+
+ return status;
+}
+
+uint8_t MX28::SetReturnDelayTime(uint8_t servoId, uint8_t returnDelayTime, bool isRegWrite)
+{
+ MX28_PROTOCOL_PACKET packet;
+
+ packet.servoId = servoId;
+ packet.length = 4;
+ packet.instructionErrorId = (isRegWrite == true) ? MX28_REG_WRITE : MX28_WRITE_DATA;
+ packet.parameter[0] = MX28_RETURN_DELAY_TIME;
+ packet.parameter[1] = returnDelayTime;
+
+ #ifdef MX28_DEBUG
+ pc->printf("Set return delay time: 0x%02X\r\n", returnDelayTime);
+ #endif
+
+ return CommunicatePacket(&packet);
+}
+
+uint8_t MX28::GetCWAngleLimit(uint8_t servoId, uint16_t *cwAngleLimit)
+{
+ MX28_PROTOCOL_PACKET packet;
+
+ packet.servoId = servoId;
+ packet.length = 4;
+ packet.instructionErrorId = MX28_READ_DATA;
+ packet.parameter[0] = MX28_CW_ANGLE_LIMIT_L;
+ packet.parameter[1] = 0x02;
+
+ uint8_t status = CommunicatePacket(&packet);
+
+ if(status == MX28_ERRBIT_NONE)
+ {
+ *cwAngleLimit = Utilities::ConvertUInt8ArrayToUInt16(packet.parameter);
+
+ #ifdef MX28_DEBUG
+ pc->printf("Set CW angle limit: %hu\r\n", *cwAngleLimit);
+ #endif
+ }
+
+ return status;
+}
+
+uint8_t MX28::SetCWAngleLimit(uint8_t servoId, uint16_t cwAngleLimit, bool isRegWrite)
+{
+ MX28_PROTOCOL_PACKET packet;
+
+ packet.servoId = servoId;
+ packet.length = 5;
+ packet.instructionErrorId = (isRegWrite == true) ? MX28_REG_WRITE : MX28_WRITE_DATA;
+ packet.parameter[0] = MX28_CW_ANGLE_LIMIT_L;
+ Utilities::ConvertInt16ToUInt8Array(cwAngleLimit, (uint8_t*)&(packet.parameter[1]));
+
+ #ifdef MX28_DEBUG
+ pc->printf("Set Cw angle limit: %hu\r\n", cwAngleLimit);
+ #endif
+
+ return CommunicatePacket(&packet);
+}
+
+uint8_t MX28::GetCCWAngleLimit(uint8_t servoId, uint16_t *ccwAngleLimit)
+{
+ MX28_PROTOCOL_PACKET packet;
+
+ packet.servoId = servoId;
+ packet.length = 4;
+ packet.instructionErrorId = MX28_READ_DATA;
+ packet.parameter[0] = MX28_CCW_ANGLE_LIMIT_L;
+ packet.parameter[1] = 0x02;
+
+ uint8_t status = CommunicatePacket(&packet);
+
+ if(status == MX28_ERRBIT_NONE)
+ {
+ *ccwAngleLimit = Utilities::ConvertUInt8ArrayToUInt16(packet.parameter);
+
+ #ifdef MX28_DEBUG
+ pc->printf("Get CCW angle limit: %hu\r\n", *ccwAngleLimit);
+ #endif
+ }
+
+ return status;
+}
+
+uint8_t MX28::SetCCWAngleLimit(uint8_t servoId, uint16_t ccwAngleLimit, bool isRegWrite)
+{
+ MX28_PROTOCOL_PACKET packet;
+
+ packet.servoId = servoId;
+ packet.length = 5;
+ packet.instructionErrorId = (isRegWrite == true) ? MX28_REG_WRITE : MX28_WRITE_DATA;
+ packet.parameter[0] = MX28_CCW_ANGLE_LIMIT_L;
+ Utilities::ConvertUInt16ToUInt8Array(ccwAngleLimit, (uint8_t*)&(packet.parameter[1]));
+
+ #ifdef MX28_DEBUG
+ pc->printf("Set CCW angle limit: %hu\r\n", ccwAngleLimit);
+ #endif
+
+ return CommunicatePacket(&packet);
+}
+
+uint8_t MX28::GetHighestTemperatureLimit(uint8_t servoId, uint8_t *highestTemperatureLimit)
+{
+ MX28_PROTOCOL_PACKET packet;
+
+ packet.servoId = servoId;
+ packet.length = 4;
+ packet.instructionErrorId = MX28_READ_DATA;
+ packet.parameter[0] = MX28_UP_LIMIT_TEMPERATURE;
+ packet.parameter[1] = 0x01;
+
+ uint8_t status = CommunicatePacket(&packet);
+
+ if(status == MX28_ERRBIT_NONE)
+ {
+ *highestTemperatureLimit = packet.parameter[0];
+
+ #ifdef MX28_DEBUG
+ pc->printf("Get highest temperature limit: 0x%02X\r\n", *highestTemperatureLimit);
+ #endif
+ }
+
+ return status;
+}
+
+uint8_t MX28::SetHighestTemperatureLimit(uint8_t servoId, uint8_t highestTemperatureLimit, bool isRegWrite)
+{
+ MX28_PROTOCOL_PACKET packet;
+
+ packet.servoId = servoId;
+ packet.length = 4;
+ packet.instructionErrorId = (isRegWrite == true) ? MX28_REG_WRITE : MX28_WRITE_DATA;
+ packet.parameter[0] = MX28_UP_LIMIT_TEMPERATURE;
+ packet.parameter[1] = highestTemperatureLimit;
+
+ #ifdef MX28_DEBUG
+ pc->printf("Set highest temperature limit: 0x%02X\r\n", highestTemperatureLimit);
+ #endif
+
+ return CommunicatePacket(&packet);
+}
+
+uint8_t MX28::GetLowestVoltageLimit(uint8_t servoId, uint8_t *lowestVoltageLimit)
+{
+ MX28_PROTOCOL_PACKET packet;
+
+ packet.servoId = servoId;
+ packet.length = 4;
+ packet.instructionErrorId = MX28_READ_DATA;
+ packet.parameter[0] = MX28_DOWN_LIMIT_VOLTAGE;
+ packet.parameter[1] = 0x01;
+
+ uint8_t status = CommunicatePacket(&packet);
+
+ if(status == MX28_ERRBIT_NONE)
+ {
+ *lowestVoltageLimit = packet.parameter[0];
+
+ #ifdef MX28_DEBUG
+ pc->printf("Get lowest voltage limit: 0x%02X\r\n", *lowestVoltageLimit);
+ #endif
+ }
+
+ return status;
+}
+
+uint8_t MX28::SetLowestVoltageLimit(uint8_t servoId, uint8_t lowestVoltageLimit, bool isRegWrite)
+{
+ MX28_PROTOCOL_PACKET packet;
+
+ packet.servoId = servoId;
+ packet.length = 4;
+ packet.instructionErrorId = (isRegWrite == true) ? MX28_REG_WRITE : MX28_WRITE_DATA;
+ packet.parameter[0] = MX28_DOWN_LIMIT_VOLTAGE;
+ packet.parameter[1] = lowestVoltageLimit;
+
+ #ifdef MX28_DEBUG
+ pc->printf("Set lowest voltage limit: 0x%02X\r\n", lowestVoltageLimit);
+ #endif
+
+ return CommunicatePacket(&packet);
+}
+
+uint8_t MX28::GetHighestVoltageLimit(uint8_t servoId, uint8_t *highestVoltageLimit)
+{
+ MX28_PROTOCOL_PACKET packet;
+
+ packet.servoId = servoId;
+ packet.length = 4;
+ packet.instructionErrorId = MX28_READ_DATA;
+ packet.parameter[0] = MX28_UP_LIMIT_VOLTAGE;
+ packet.parameter[1] = 0x01;
+
+ uint8_t status = CommunicatePacket(&packet);
+
+ if(status == MX28_ERRBIT_NONE)
+ {
+ *highestVoltageLimit = packet.parameter[0];
+
+ #ifdef MX28_DEBUG
+ pc->printf("Get highest voltage limit: 0x%02X\r\n", *highestVoltageLimit);
+ #endif
+ }
+
+ return status;
+}
+
+uint8_t MX28::SetHighestVoltageLimit(uint8_t servoId, uint8_t highestVoltageLimit, bool isRegWrite)
+{
+ MX28_PROTOCOL_PACKET packet;
+
+ packet.servoId = servoId;
+ packet.length = 4;
+ packet.instructionErrorId = (isRegWrite == true) ? MX28_REG_WRITE : MX28_WRITE_DATA;
+ packet.parameter[0] = MX28_UP_LIMIT_VOLTAGE;
+ packet.parameter[1] = highestVoltageLimit;
+
+ #ifdef MX28_DEBUG
+ pc->printf("Set highest voltage limit: 0x%02X\r\n", highestVoltageLimit);
+ #endif
+
+ return CommunicatePacket(&packet);
+}
+
+uint8_t MX28::GetMaxTorque(uint8_t servoId, uint16_t *maxTorque)
+{
+ MX28_PROTOCOL_PACKET packet;
+
+ packet.servoId = servoId;
+ packet.length = 4;
+ packet.instructionErrorId = MX28_READ_DATA;
+ packet.parameter[0] = MX28_MAX_TORQUE_L;
+ packet.parameter[1] = 0x02;
+
+ uint8_t status = CommunicatePacket(&packet);
+
+ if(status == MX28_ERRBIT_NONE)
+ {
+ *maxTorque = Utilities::ConvertUInt8ArrayToUInt16(packet.parameter);
+
+ #ifdef MX28_DEBUG
+ pc->printf("Get max torque: %hu\r\n", *maxTorque);
+ #endif
+ }
+
+ return status;
+}
+
+uint8_t MX28::SetMaxTorque(uint8_t servoId, uint16_t maxTorque, bool isRegWrite)
+{
+ MX28_PROTOCOL_PACKET packet;
+
+ packet.servoId = servoId;
+ packet.length = 5;
+ packet.instructionErrorId = (isRegWrite == true) ? MX28_REG_WRITE : MX28_WRITE_DATA;
+ packet.parameter[0] = MX28_MAX_TORQUE_L;
+ Utilities::ConvertUInt16ToUInt8Array(maxTorque, (uint8_t*)&(packet.parameter[1]));
+
+ #ifdef MX28_DEBUG
+ pc->printf("Set max torque: %hu\r\n", maxTorque);
+ #endif
+
+ return CommunicatePacket(&packet);
+}
+
+uint8_t MX28::GetStatusReturnLevel(uint8_t servoId, uint8_t *statusReturnLevel)
+{
+ MX28_PROTOCOL_PACKET packet;
+
+ packet.servoId = servoId;
+ packet.length = 4;
+ packet.instructionErrorId = MX28_READ_DATA;
+ packet.parameter[0] = MX28_STATUS_RETURN_LEVEL;
+ packet.parameter[1] = 0x01;
+
+ uint8_t status = CommunicatePacket(&packet);
+
+ if(status == MX28_ERRBIT_NONE)
+ {
+ *statusReturnLevel = packet.parameter[0];
+
+ #ifdef MX28_DEBUG
+ pc->printf("Get status return level: 0x%02X\r\n", *statusReturnLevel);
+ #endif
+ }
+
+ return status;
+}
+
+uint8_t MX28::SetStatusReturnLevel(uint8_t servoId, uint8_t statusReturnLevel, bool isRegWrite)
+{
+ MX28_PROTOCOL_PACKET packet;
+
+ packet.servoId = servoId;
+ packet.length = 4;
+ packet.instructionErrorId = (isRegWrite == true) ? MX28_REG_WRITE : MX28_WRITE_DATA;
+ packet.parameter[0] = MX28_STATUS_RETURN_LEVEL;
+ packet.parameter[1] = statusReturnLevel;
+
+ #ifdef MX28_DEBUG
+ pc->printf("Set status return level: 0x%02X\r\n", statusReturnLevel);
+ #endif
+
+ return CommunicatePacket(&packet);
+}
+
+uint8_t MX28::GetAlarmLED(uint8_t servoId, uint8_t *alarmLED)
+{
+ MX28_PROTOCOL_PACKET packet;
+
+ packet.servoId = servoId;
+ packet.length = 4;
+ packet.instructionErrorId = MX28_READ_DATA;
+ packet.parameter[0] = MX28_ALARM_LED;
+ packet.parameter[1] = 0x01;
+
+ uint8_t status = CommunicatePacket(&packet);
+
+ if(status == MX28_ERRBIT_NONE)
+ {
+ *alarmLED = packet.parameter[0];
+
+ #ifdef MX28_DEBUG
+ pc->printf("Get alarm LED: 0x%02X\r\n", *alarmLED);
+ #endif
+ }
+
+ return status;
+}
+
+uint8_t MX28::SetAlarmLED(uint8_t servoId, uint8_t alarmLED, bool isRegWrite)
+{
+ MX28_PROTOCOL_PACKET packet;
+
+ packet.servoId = servoId;
+ packet.length = 4;
+ packet.instructionErrorId = (isRegWrite == true) ? MX28_REG_WRITE : MX28_WRITE_DATA;
+ packet.parameter[0] = MX28_ALARM_LED;
+ packet.parameter[1] = alarmLED;
+
+ #ifdef MX28_DEBUG
+ pc->printf("Set alarm LED: 0x%02X\r\n", alarmLED);
+ #endif
+
+ return CommunicatePacket(&packet);
+}
+
+uint8_t MX28::GetAlarmShutdown(uint8_t servoId, uint8_t *alarmShutdown)
+{
+ MX28_PROTOCOL_PACKET packet;
+
+ packet.servoId = servoId;
+ packet.length = 4;
+ packet.instructionErrorId = MX28_READ_DATA;
+ packet.parameter[0] = MX28_ALARM_SHUTDOWN;
+ packet.parameter[1] = 0x01;
+
+ uint8_t status = CommunicatePacket(&packet);
+
+ if(status == MX28_ERRBIT_NONE)
+ {
+ *alarmShutdown = packet.parameter[0];
+
+ #ifdef MX28_DEBUG
+ pc->printf("Get alarm shutdown: 0x%02X\r\n", *alarmShutdown);
+ #endif
+ }
+
+ return status;
+}
+
+uint8_t MX28::SetAlarmShutdown(uint8_t servoId, uint8_t alarmShutdown, bool isRegWrite)
+{
+ MX28_PROTOCOL_PACKET packet;
+
+ packet.servoId = servoId;
+ packet.length = 4;
+ packet.instructionErrorId = (isRegWrite == true) ? MX28_REG_WRITE : MX28_WRITE_DATA;
+ packet.parameter[0] = MX28_ALARM_SHUTDOWN;
+ packet.parameter[1] = alarmShutdown;
+
+ #ifdef MX28_DEBUG
+ pc->printf("Set alarm shutdown: 0x%02X\r\n", alarmShutdown);
+ #endif
+
+ return CommunicatePacket(&packet);
+}
+
+uint8_t MX28::GetEnableTorque(uint8_t servoId, uint8_t *enableTorque)
+{
+ MX28_PROTOCOL_PACKET packet;
+
+ packet.servoId = servoId;
+ packet.length = 4;
+ packet.instructionErrorId = MX28_READ_DATA;
+ packet.parameter[0] = MX28_TORQUE_ENABLE;
+ packet.parameter[1] = 0x01;
+
+ uint8_t status = CommunicatePacket(&packet);
+
+ if(status == MX28_ERRBIT_NONE)
+ {
+ *enableTorque = packet.parameter[0];
+
+ #ifdef MX28_DEBUG
+ pc->printf("Get enable torque: 0x%02X\r\n", *enableTorque);
+ #endif
+ }
+
+ return status;
+}
+
+uint8_t MX28::SetEnableTorque(uint8_t servoId, uint8_t enableTorque, bool isRegWrite)
+{
+ MX28_PROTOCOL_PACKET packet;
+
+ packet.servoId = servoId;
+ packet.length = 4;
+ packet.instructionErrorId = (isRegWrite == true) ? MX28_REG_WRITE : MX28_WRITE_DATA;
+ packet.parameter[0] = MX28_TORQUE_ENABLE;
+ packet.parameter[1] = enableTorque;
+
+ #ifdef MX28_DEBUG
+ pc->printf("Set enable torque: 0x%02X\r\n", enableTorque);
+ #endif
+
+ return CommunicatePacket(&packet);
+}
+
+uint8_t MX28::GetEnableLED(uint8_t servoId, uint8_t *enableLED)
+{
+ MX28_PROTOCOL_PACKET packet;
+
+ packet.servoId = servoId;
+ packet.length = 4;
+ packet.instructionErrorId = MX28_READ_DATA;
+ packet.parameter[0] = MX28_LED_ENABLE;
+ packet.parameter[1] = 0x01;
+
+ uint8_t status = CommunicatePacket(&packet);
+
+ if(status == MX28_ERRBIT_NONE)
+ {
+ *enableLED = packet.parameter[0];
+
+ #ifdef MX28_DEBUG
+ pc->printf("Get enable LED: 0x%02X\r\n", *enableLED);
+ #endif
+ }
+
+ return status;
+}
+
+uint8_t MX28::SetEnableLED(uint8_t servoId, uint8_t enableLED, bool isRegWrite)
+{
+ MX28_PROTOCOL_PACKET packet;
+
+ packet.servoId = servoId;
+ packet.length = 4;
+ packet.instructionErrorId = (isRegWrite == true) ? MX28_REG_WRITE : MX28_WRITE_DATA;
+ packet.parameter[0] = MX28_LED_ENABLE;
+ packet.parameter[1] = enableLED;
+
+ #ifdef MX28_DEBUG
+ pc->printf("Set enable LED: 0x%02X\r\n", enableLED);
+ #endif
+
+ return CommunicatePacket(&packet);
+}
+
+uint8_t MX28::GetPGain(uint8_t servoId, uint8_t *pGain)
+{
+ MX28_PROTOCOL_PACKET packet;
+
+ packet.servoId = servoId;
+ packet.length = 4;
+ packet.instructionErrorId = MX28_READ_DATA;
+ packet.parameter[0] = MX28_P_GAIN;
+ packet.parameter[1] = 0x01;
+
+ uint8_t status = CommunicatePacket(&packet);
+
+ if(status == MX28_ERRBIT_NONE)
+ {
+ *pGain = packet.parameter[0];
+
+ #ifdef MX28_DEBUG
+ pc->printf("Get P gain: 0x%02X\r\n", *pGain);
+ #endif
+ }
+
+ return status;
+}
+
+uint8_t MX28::SetPGain(uint8_t servoId, uint8_t pGain, bool isRegWrite)
+{
+ MX28_PROTOCOL_PACKET packet;
+
+ packet.servoId = servoId;
+ packet.length = 4;
+ packet.instructionErrorId = (isRegWrite == true) ? MX28_REG_WRITE : MX28_WRITE_DATA;
+ packet.parameter[0] = MX28_P_GAIN;
+ packet.parameter[1] = pGain;
+
+ #ifdef MX28_DEBUG
+ pc->printf("Set P gain: 0x%02X\r\n", pGain);
+ #endif
+
+ return CommunicatePacket(&packet);
+}
+
+uint8_t MX28::GetIGain(uint8_t servoId, uint8_t *iGain)
+{
+ MX28_PROTOCOL_PACKET packet;
+
+ packet.servoId = servoId;
+ packet.length = 4;
+ packet.instructionErrorId = MX28_READ_DATA;
+ packet.parameter[0] = MX28_I_GAIN;
+ packet.parameter[1] = 0x01;
+
+ uint8_t status = CommunicatePacket(&packet);
+
+ if(status == MX28_ERRBIT_NONE)
+ {
+ *iGain = packet.parameter[0];
+
+ #ifdef MX28_DEBUG
+ pc->printf("Get I gain: 0x%02X\r\n", *iGain);
+ #endif
+ }
+
+ return status;
+}
+
+uint8_t MX28::SetIGain(uint8_t servoId, uint8_t iGain, bool isRegWrite)
+{
+ MX28_PROTOCOL_PACKET packet;
+
+ packet.servoId = servoId;
+ packet.length = 4;
+ packet.instructionErrorId = (isRegWrite == true) ? MX28_REG_WRITE : MX28_WRITE_DATA;
+ packet.parameter[0] = MX28_I_GAIN;
+ packet.parameter[1] = iGain;
+
+ #ifdef MX28_DEBUG
+ pc->printf("Set I gain: 0x%02X\r\n", iGain);
+ #endif
+
+ return CommunicatePacket(&packet);
+}
+
+uint8_t MX28::GetDGain(uint8_t servoId, uint8_t *dGain)
+{
+ MX28_PROTOCOL_PACKET packet;
+
+ packet.servoId = servoId;
+ packet.length = 4;
+ packet.instructionErrorId = MX28_READ_DATA;
+ packet.parameter[0] = MX28_D_GAIN;
+ packet.parameter[1] = 0x01;
+
+ uint8_t status = CommunicatePacket(&packet);
+
+ if(status == MX28_ERRBIT_NONE)
+ {
+ *dGain = packet.parameter[0];
+
+ #ifdef MX28_DEBUG
+ pc->printf("Get D gain: 0x%02X\r\n", *dGain);
+ #endif
+ }
+
+ return status;
+}
+
+uint8_t MX28::SetDGain(uint8_t servoId, uint8_t dGain, bool isRegWrite)
+{
+ MX28_PROTOCOL_PACKET packet;
+
+ packet.servoId = servoId;
+ packet.length = 4;
+ packet.instructionErrorId = (isRegWrite == true) ? MX28_REG_WRITE : MX28_WRITE_DATA;
+ packet.parameter[0] = MX28_D_GAIN;
+ packet.parameter[1] = dGain;
+
+ #ifdef MX28_DEBUG
+ pc->printf("Set D gain: 0x%02X\r\n", dGain);
+ #endif
+
+ return CommunicatePacket(&packet);
+}
+
+uint8_t MX28::GetGoalPosition(uint8_t servoId, uint16_t *goalPosition)
+{
+ MX28_PROTOCOL_PACKET packet;
+
+ packet.servoId = servoId;
+ packet.length = 4;
+ packet.instructionErrorId = MX28_READ_DATA;
+ packet.parameter[0] = MX28_GOAL_POSITION_L;
+ packet.parameter[1] = 0x02;
+
+ uint8_t status = CommunicatePacket(&packet);
+
+ if(status == MX28_ERRBIT_NONE)
+ {
+ *goalPosition = Utilities::ConvertUInt8ArrayToUInt16(packet.parameter);
+
+ #ifdef MX28_DEBUG
+ pc->printf("Get goal position: %hu\r\n", *goalPosition);
+ #endif
+ }
+
+ return status;
+}
+
+uint8_t MX28::SetGoalPosition(uint8_t servoId, uint16_t goalPosition, bool isRegWrite)
+{
+ MX28_PROTOCOL_PACKET packet;
+
+ packet.servoId = servoId;
+ packet.length = 5;
+ packet.instructionErrorId = (isRegWrite == true) ? MX28_REG_WRITE : MX28_WRITE_DATA;
+ packet.parameter[0] = MX28_GOAL_POSITION_L;
+ Utilities::ConvertUInt16ToUInt8Array(goalPosition, (uint8_t*)&(packet.parameter[1]));
+
+ #ifdef MX28_DEBUG
+ pc->printf("Set goal position: %hu\r\n", goalPosition);
+ #endif
+
+ return CommunicatePacket(&packet);
+}
+
+uint8_t MX28::GetMovingSpeed(uint8_t servoId, uint16_t *movingSpeed)
+{
+ MX28_PROTOCOL_PACKET packet;
+
+ packet.servoId = servoId;
+ packet.length = 4;
+ packet.instructionErrorId = MX28_READ_DATA;
+ packet.parameter[0] = MX28_MOVING_SPEED_L;
+ packet.parameter[1] = 0x02;
+
+ uint8_t status = CommunicatePacket(&packet);
+
+ if(status == MX28_ERRBIT_NONE)
+ {
+ *movingSpeed = Utilities::ConvertUInt8ArrayToUInt16(packet.parameter);
+
+ #ifdef MX28_DEBUG
+ pc->printf("Get moving speed: %hu\r\n", *movingSpeed);
+ #endif
+ }
+
+ return status;
+}
+
+uint8_t MX28::SetMovingSpeed(uint8_t servoId, uint16_t movingSpeed, bool isRegWrite)
+{
+ MX28_PROTOCOL_PACKET packet;
+
+ packet.servoId = servoId;
+ packet.length = 5;
+ packet.instructionErrorId = (isRegWrite == true) ? MX28_REG_WRITE : MX28_WRITE_DATA;
+ packet.parameter[0] = MX28_MOVING_SPEED_L;
+ Utilities::ConvertUInt16ToUInt8Array(movingSpeed, (uint8_t*)&(packet.parameter[1]));
+
+ #ifdef MX28_DEBUG
+ pc->printf("Set moving speed: %hu\r\n", movingSpeed);
+ #endif
+
+ return CommunicatePacket(&packet);
+}
+
+uint8_t MX28::GetTorqueLimit(uint8_t servoId, uint16_t *torqueLimit)
+{
+ MX28_PROTOCOL_PACKET packet;
+
+ packet.servoId = servoId;
+ packet.length = 4;
+ packet.instructionErrorId = MX28_READ_DATA;
+ packet.parameter[0] = MX28_TORQUE_LIMIT_L;
+ packet.parameter[1] = 0x02;
+
+ uint8_t status = CommunicatePacket(&packet);
+
+ if(status == MX28_ERRBIT_NONE)
+ {
+ *torqueLimit = Utilities::ConvertUInt8ArrayToUInt16(packet.parameter);
+
+ #ifdef MX28_DEBUG
+ pc->printf("Torque limit: %hu\r\n", *torqueLimit);
+ #endif
+ }
+
+ return status;
+}
+
+uint8_t MX28::SetTorqueLimit(uint8_t servoId, uint16_t torqueLimit, bool isRegWrite)
+{
+ MX28_PROTOCOL_PACKET packet;
+
+ packet.servoId = servoId;
+ packet.length = 5;
+ packet.instructionErrorId = (isRegWrite == true) ? MX28_REG_WRITE : MX28_WRITE_DATA;
+ packet.parameter[0] = MX28_TORQUE_LIMIT_L;
+ Utilities::ConvertUInt16ToUInt8Array(torqueLimit, (uint8_t*)&(packet.parameter[1]));
+
+ #ifdef MX28_DEBUG
+ pc->printf("Set torque limit: %hu\r\n", torqueLimit);
+ #endif
+
+ return CommunicatePacket(&packet);
+}
+
+uint8_t MX28::GetPresentPosition(uint8_t servoId, uint16_t *presentPosition)
+{
+ MX28_PROTOCOL_PACKET packet;
+
+ packet.servoId = servoId;
+ packet.length = 4;
+ packet.instructionErrorId = MX28_READ_DATA;
+ packet.parameter[0] = MX28_PRESENT_POSITION_L;
+ packet.parameter[1] = 0x02;
+
+ uint8_t status = CommunicatePacket(&packet);
+
+ if(status == MX28_ERRBIT_NONE)
+ {
+ *presentPosition = Utilities::ConvertUInt8ArrayToUInt16(packet.parameter);
+
+ #ifdef MX28_DEBUG
+ pc->printf("Get present position: %hu\r\n", *presentPosition);
+ #endif
+ }
+
+ return status;
+}
+
+uint8_t MX28::GetPresentSpeed(uint8_t servoId, uint16_t *presentSpeed)
+{
+ MX28_PROTOCOL_PACKET packet;
+
+ packet.servoId = servoId;
+ packet.length = 4;
+ packet.instructionErrorId = MX28_READ_DATA;
+ packet.parameter[0] = MX28_PRESENT_SPEED_L;
+ packet.parameter[1] = 0x02;
+
+ uint8_t status = CommunicatePacket(&packet);
+
+ if(status == MX28_ERRBIT_NONE)
+ {
+ *presentSpeed = Utilities::ConvertUInt8ArrayToUInt16(packet.parameter);
+
+ #ifdef MX28_DEBUG
+ pc->printf("Get present speed: %hu\r\n", *presentSpeed);
+ #endif
+ }
+
+ return status;
+}
+
+uint8_t MX28::GetPresentLoad(uint8_t servoId, uint16_t *presentLoad)
+{
+ MX28_PROTOCOL_PACKET packet;
+
+ packet.servoId = servoId;
+ packet.length = 4;
+ packet.instructionErrorId = MX28_READ_DATA;
+ packet.parameter[0] = MX28_PRESENT_LOAD_L;
+ packet.parameter[1] = 0x02;
+
+ uint8_t status = CommunicatePacket(&packet);
+
+ if(status == MX28_ERRBIT_NONE)
+ {
+ *presentLoad = Utilities::ConvertUInt8ArrayToUInt16(packet.parameter);
+
+ #ifdef MX28_DEBUG
+ pc->printf("Get present load: %hu\r\n", *presentLoad);
+ #endif
+ }
+
+ return status;
+}
+
+uint8_t MX28::GetPresentVoltage(uint8_t servoId, uint8_t *presentVoltage)
+{
+ MX28_PROTOCOL_PACKET packet;
+
+ packet.servoId = servoId;
+ packet.length = 4;
+ packet.instructionErrorId = MX28_READ_DATA;
+ packet.parameter[0] = MX28_PRESENT_VOLTAGE;
+ packet.parameter[1] = 0x01;
+
+ uint8_t status = CommunicatePacket(&packet);
+
+ if(status == MX28_ERRBIT_NONE)
+ {
+ *presentVoltage = packet.parameter[0];
+
+ #ifdef MX28_DEBUG
+ pc->printf("Get present voltage: 0x%02X\r\n", *presentVoltage);
+ #endif
+ }
+
+ return status;
+}
+
+uint8_t MX28::GetPresentTemperature(uint8_t servoId, uint8_t *presentTemperature)
+{
+ MX28_PROTOCOL_PACKET packet;
+
+ packet.servoId = servoId;
+ packet.length = 4;
+ packet.instructionErrorId = MX28_READ_DATA;
+ packet.parameter[0] = MX28_PRESENT_TEMPERATURE;
+ packet.parameter[1] = 0x01;
+
+ uint8_t status = CommunicatePacket(&packet);
+
+ if(status == MX28_ERRBIT_NONE)
+ {
+ *presentTemperature = packet.parameter[0];
+
+ #ifdef MX28_DEBUG
+ pc->printf("Get present temperature: 0x%02X\r\n", *presentTemperature);
+ #endif
+ }
+
+ return status;
+}
+
+uint8_t MX28::GetIsRegistered(uint8_t servoId, uint8_t *registered)
+{
+ MX28_PROTOCOL_PACKET packet;
+
+ packet.servoId = servoId;
+ packet.length = 4;
+ packet.instructionErrorId = MX28_READ_DATA;
+ packet.parameter[0] = MX28_PRESENT_TEMPERATURE;
+ packet.parameter[1] = 0x01;
+
+ uint8_t status = CommunicatePacket(&packet);
+
+ if(status == MX28_ERRBIT_NONE)
+ {
+ *registered = packet.parameter[0];
+
+ #ifdef MX28_DEBUG
+ pc->printf("Get is registered: 0x%02X\r\n", *registered);
+ #endif
+ }
+
+ return status;
+}
+
+uint8_t MX28::GetIsMoving(uint8_t servoId, uint8_t *moving)
+{
+ MX28_PROTOCOL_PACKET packet;
+
+ packet.servoId = servoId;
+ packet.length = 4;
+ packet.instructionErrorId = MX28_READ_DATA;
+ packet.parameter[0] = MX28_PRESENT_TEMPERATURE;
+ packet.parameter[1] = 0x01;
+
+ uint8_t status = CommunicatePacket(&packet);
+
+ if(status == MX28_ERRBIT_NONE)
+ {
+ *moving = packet.parameter[0];
+
+ #ifdef MX28_DEBUG
+ pc->printf("Get is moving: 0x%02X\r\n", *moving);
+ #endif
+ }
+
+ return status;
+}
+
+uint8_t MX28::GetIsLocked(uint8_t servoId, uint8_t *isLocked)
+{
+ MX28_PROTOCOL_PACKET packet;
+
+ packet.servoId = servoId;
+ packet.length = 4;
+ packet.instructionErrorId = MX28_READ_DATA;
+ packet.parameter[0] = MX28_LOCK;
+ packet.parameter[1] = 0x01;
+
+ uint8_t status = CommunicatePacket(&packet);
+
+ if(status == MX28_ERRBIT_NONE)
+ {
+ *isLocked = packet.parameter[0];
+
+ #ifdef MX28_DEBUG
+ pc->printf("Get is locked: 0x%02X\r\n", *isLocked);
+ #endif
+ }
+
+ return status;
+}
+
+uint8_t MX28::SetIsLocked(uint8_t servoId, uint8_t isLocked, bool isRegWrite)
+{
+ MX28_PROTOCOL_PACKET packet;
+
+ packet.servoId = servoId;
+ packet.length = 4;
+ packet.instructionErrorId = (isRegWrite == true) ? MX28_REG_WRITE : MX28_WRITE_DATA;
+ packet.parameter[0] = MX28_LOCK;
+ packet.parameter[1] = isLocked;
+
+ #ifdef MX28_DEBUG
+ pc->printf("Set is locked: 0x%02X\r\n", isLocked);
+ #endif
+
+ return CommunicatePacket(&packet);
+}
+
+uint8_t MX28::GetPunch(uint8_t servoId, uint16_t *punch)
+{
+ MX28_PROTOCOL_PACKET packet;
+
+ packet.servoId = servoId;
+ packet.length = 4;
+ packet.instructionErrorId = MX28_READ_DATA;
+ packet.parameter[0] = MX28_PUNCH_L;
+ packet.parameter[1] = 0x02;
+
+ uint8_t status = CommunicatePacket(&packet);
+
+ if(status == MX28_ERRBIT_NONE)
+ {
+ *punch = Utilities::ConvertUInt8ArrayToUInt16(packet.parameter);
+
+ #ifdef MX28_DEBUG
+ pc->printf("Get punch: %hu\r\n", *punch);
+ #endif
+ }
+
+ return status;
+}
+
+uint8_t MX28::SetPunch(uint8_t servoId, uint16_t punch, bool isRegWrite)
+{
+ MX28_PROTOCOL_PACKET packet;
+
+ packet.servoId = servoId;
+ packet.length = 5;
+ packet.instructionErrorId = (isRegWrite == true) ? MX28_REG_WRITE : MX28_WRITE_DATA;
+ packet.parameter[0] = MX28_PUNCH_L;
+ Utilities::ConvertUInt16ToUInt8Array(punch, (uint8_t*)&(packet.parameter[1]));
+
+ #ifdef MX28_DEBUG
+ pc->printf("Set punch: %hu\r\n", punch);
+ #endif
+
+ return CommunicatePacket(&packet);
+}
+
+uint8_t MX28::Ping(uint8_t servoId)
+{
+ MX28_PROTOCOL_PACKET packet;
+
+ packet.servoId = servoId;
+ packet.length = 2;
+ packet.instructionErrorId = MX28_PING;
+
+ #ifdef MX28_DEBUG
+ pc->printf("Ping\r\n");
+ #endif
+
+ return CommunicatePacket(&packet);
+}
+
+uint8_t MX28::Reset(uint8_t servoId)
+{
+ MX28_PROTOCOL_PACKET packet;
+
+ packet.servoId = servoId;
+ packet.length = 2;
+ packet.instructionErrorId = MX28_RESET;
+
+ #ifdef MX28_DEBUG
+ pc->printf("Reset\r\n");
+ #endif
+
+ return CommunicatePacket(&packet);
+}
+
+uint8_t MX28::Action(uint8_t servoId)
+{
+ MX28_PROTOCOL_PACKET packet;
+
+ packet.servoId = servoId;
+ packet.length = 2;
+ packet.instructionErrorId = MX28_ACTION;
+
+ #ifdef MX28_DEBUG
+ pc->printf("Action\r\n");
+ #endif
+
+ return CommunicatePacket(&packet);
+}
+
+uint8_t MX28::SyncWrite(uint8_t *data, uint8_t length)
+{
+ MX28_PROTOCOL_PACKET packet;
+
+ packet.servoId = MX28_PROTOCOL_BROADCAST_ID;
+ packet.length = length;
+ packet.instructionErrorId = MX28_SYNC_WRITE;
+ memcpy(packet.parameter, &data, sizeof(uint8_t) * length);
+
+ #ifdef MX28_DEBUG
+ pc->printf("SyncWrite\r\n");
+ #endif
+
+ return CommunicatePacket(&packet);
+}
+
+MX28::MX28(PinName tx, PinName rx, int baudRate)
+{
+ #ifdef MX28_DEBUG
+ pc = new Serial(USBTX, USBRX);
+ pc->baud(115200);
+ pc->printf("\033[2J");
+ #endif
+
+ servoSerialHalfDuplex = new SerialHalfDuplex(tx, rx);
+ servoSerialHalfDuplex->baud(baudRate);
+}
+
+MX28::~MX28()
+{
+ #ifdef MX28_DEBUG
+ if(pc != NULL)
+ delete pc;
+ #endif
+
+ if(servoSerialHalfDuplex != NULL)
+ delete servoSerialHalfDuplex;
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/MX28.h Sun Sep 09 22:09:00 2012 +0000
@@ -0,0 +1,714 @@
+/* Dynamixel MX28 servo library
+ * Copyright (c) 2012 Georgios Petrou, MIT License
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy of this software
+ * and associated documentation files (the "Software"), to deal in the Software without restriction,
+ * including without limitation the rights to use, copy, modify, merge, publish, distribute,
+ * sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all copies or
+ * substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
+ * BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
+ * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
+
+#ifndef MX28_H
+#define MX28_H
+
+#include "mbed.h"
+#include "Protocol.h"
+#include "Utilities.h"
+
+#define MX28_DEBUG
+
+/** MX28 servo control class
+*
+* Example:
+* @code
+*
+* #include "mbed.h"
+* #include "MX28.h"
+*
+* Serial pc(USBTX, USBRX);
+* MX28 mx28(p28, p27, 57600);
+*
+* int main()
+* {
+* pc.baud(115200);
+*
+* pc.getc();
+* pc.printf("======================================================\r\n");
+*
+* uint8_t servoId = 0x01;
+*
+* uint16_t modelNumber;
+* mx28.GetModelNumber(servoId, &modelNumber);
+*
+* uint8_t firmwareVersion;
+* mx28.GetFirmwareVersion(servoId, &firmwareVersion);
+*
+* uint8_t id;
+* mx28.GetId(servoId, &id);
+* mx28.SetId(servoId, servoId);
+*
+* int32_t baudRate;
+* mx28.GetBaudRate(servoId, &baudRate);
+* mx28.SetBaudRate(servoId, 57600);
+*
+* uint8_t returnDelayTime;
+* mx28.GetReturnDelayTime(servoId, &returnDelayTime);
+* mx28.SetReturnDelayTime(servoId, 0xFA);
+*
+* uint16_t cwAngleLimit;
+* mx28.GetCWAngleLimit(servoId, &cwAngleLimit);
+* mx28.SetCWAngleLimit(servoId, 0x0000);
+*
+* uint16_t ccwAngleLimit;
+* mx28.GetCCWAngleLimit(servoId, &ccwAngleLimit);
+* mx28.SetCCWAngleLimit(servoId, 0x0FFF);
+*
+* uint8_t highestTemperatureLimit;
+* mx28.GetHighestTemperatureLimit(servoId, &highestTemperatureLimit);
+* mx28.SetHighestTemperatureLimit(servoId, 0x50);
+*
+* uint8_t downLimitVoltage;
+* mx28.GetLowestVoltageLimit(servoId, &downLimitVoltage);
+* mx28.SetLowestVoltageLimit(servoId, 0x3C);
+*
+* uint8_t upLimitVoltage;
+* mx28.GetHighestVoltageLimit(servoId, &upLimitVoltage);
+* mx28.SetHighestVoltageLimit(servoId, 0xA0);
+*
+* uint16_t maxTorque;
+* mx28.GetMaxTorque(servoId, &maxTorque);
+* mx28.SetMaxTorque(servoId, 0x03FF);
+*
+* uint8_t statusReturnLevel;
+* mx28.GetStatusReturnLevel(servoId, &statusReturnLevel);
+* mx28.SetStatusReturnLevel(servoId, 0x02);
+*
+* uint8_t alarmLED;
+* mx28.GetAlarmLED(servoId, &alarmLED);
+* mx28.SetAlarmLED(servoId, 0x24);
+*
+* uint8_t alarmShutdown;
+* mx28.GetAlarmShutdown(servoId, &alarmShutdown);
+* mx28.SetAlarmShutdown(servoId, 0x24);
+*
+* uint8_t enableTorque;
+* mx28.GetEnableTorque(servoId, &enableTorque);
+* mx28.SetEnableTorque(servoId, 0x00);
+*
+* uint8_t enableLED;
+* mx28.GetEnableLED(servoId, &enableLED);
+* mx28.SetEnableLED(servoId, 0x00);
+*
+* uint8_t pGain;
+* mx28.GetPGain(servoId, &pGain);
+* mx28.SetPGain(servoId, 0x20);
+*
+* uint8_t iGain;
+* mx28.GetIGain(servoId, &iGain);
+* mx28.SetIGain(servoId, 0x00);
+*
+* uint8_t dGain;
+* mx28.GetDGain(servoId, &dGain);
+* mx28.SetDGain(servoId, 0x00);
+*
+* uint16_t goalPosition;
+* mx28.GetGoalPosition(servoId, &goalPosition);
+* mx28.SetGoalPosition(servoId, 0x0800);
+*
+* uint16_t movingSpeed;
+* mx28.GetMovingSpeed(servoId, &movingSpeed);
+* mx28.SetMovingSpeed(servoId, 0x00FF);
+*
+* uint16_t torqueLimit;
+* mx28.GetTorqueLimit(servoId, &torqueLimit);
+* mx28.SetTorqueLimit(servoId, 0x03FF);
+*
+* uint16_t presentPosition;
+* mx28.GetPresentPosition(servoId, &presentPosition);
+*
+* uint16_t presentSpeed;
+* mx28.GetPresentSpeed(servoId, &presentSpeed);
+*
+* uint16_t presentLoad;
+* mx28.GetPresentLoad(servoId, &presentLoad);
+*
+* uint8_t presentVoltage;
+* mx28.GetPresentVoltage(servoId, &presentVoltage);
+*
+* uint8_t presentTemperature;
+* mx28.GetPresentTemperature(servoId, &presentTemperature);
+*
+* uint8_t isRegistered;
+* mx28.GetIsRegistered(servoId, &isRegistered);
+*
+* uint8_t isMoving;
+* mx28.GetIsMoving(servoId, &isMoving);
+*
+* uint8_t lock;
+* mx28.GetIsLocked(servoId, &lock);
+* mx28.SetIsLocked(servoId, 0x00);
+*
+* uint16_t punch;
+* mx28.GetPunch(servoId, &punch);
+* mx28.SetPunch(servoId, 0x0020);
+*
+* mx28.Ping(servoId);
+*
+* mx28.Reset(servoId);
+*
+* pc.printf("======================================================\r\n");
+*
+* return 0;
+* }
+* @endcode
+*/
+class MX28
+{
+ private:
+ /** PC serial connection used in debug mode.
+ */
+ Serial *pc;
+
+ /** Servo serial half duplex connection.
+ */
+ SerialHalfDuplex *servoSerialHalfDuplex;
+
+ public:
+ /** Send the MX28 packet over the serial half duplex connection.
+ *
+ * @param packet The MX28 packet.
+ * @return MX28_ERRBIT_NONE if succeeded, error code otherwise.
+ */
+ uint8_t CommunicatePacket(MX28_PROTOCOL_PACKET *packet);
+
+ /** Get the servo model number.
+ *
+ * @param servoId The servo id.
+ * @param modelNumber The variable to store the model number.
+ * @return MX28_ERRBIT_NONE if succeeded, error code otherwise.
+ */
+ uint8_t GetModelNumber(uint8_t servoId, uint16_t *modelNumber);
+
+ /** Get the servo firmware version.
+ *
+ * @param servoId The servo id.
+ * @param firmwareVersion The variable to store the model number.
+ * @return MX28_ERRBIT_NONE if succeeded, error code otherwise.
+ */
+ uint8_t GetFirmwareVersion(uint8_t servoId, uint8_t *firmwareVersion);
+
+ /** Get the servo id.
+ *
+ * @param servoId The servo id.
+ * @param id The variable to store the id.
+ * @return MX28_ERRBIT_NONE if succeeded, error code otherwise.
+ */
+ uint8_t GetId(uint8_t servoId, uint8_t *id);
+
+ /** Set the servo id.
+ *
+ * @param servoId The servo id.
+ * @param newId The new servo id.
+ * @param isRegWrite If the command will be registered.
+ * @return MX28_ERRBIT_NONE if succeeded, error code otherwise.
+ */
+ uint8_t SetId(uint8_t servoId, uint8_t newId, bool isRegWrite = false);
+
+ /** Get the servo baudrate.
+ *
+ * @param servoId The servo id.
+ * @param baudRate The variable to store the baudrate.
+ * @return MX28_ERRBIT_NONE if succeeded, error code otherwise.
+ */
+ uint8_t GetBaudRate(uint8_t servoId, int32_t *baudRate);
+
+ /** Set the servo baudrate.
+ *
+ * @param servoId The servo id.
+ * @param baudRate The servo baudrate.
+ * @param isRegWrite If the command will be registered.
+ * @return MX28_ERRBIT_NONE if succeeded, error code otherwise.
+ */
+ uint8_t SetBaudRate(uint8_t servoId, int baudRate, bool isRegWrite = false);
+
+ /** Get the servo return delay time.
+ *
+ * @param servoId The servo id.
+ * @param returnDelayTime The variable to store the return delay time.
+ * @return MX28_ERRBIT_NONE if succeeded, error code otherwise.
+ */
+ uint8_t GetReturnDelayTime(uint8_t servoId, uint8_t *returnDelayTime);
+
+ /** Set the servo delay time.
+ *
+ * @param servoId The servo id.
+ * @param returnDelayTime The servo return delay time.
+ * @param isRegWrite If the command will be registered.
+ * @return MX28_ERRBIT_NONE if succeeded, error code otherwise.
+ */
+ uint8_t SetReturnDelayTime(uint8_t servoId, uint8_t returnDelayTime, bool isRegWrite = false);
+
+ /** Get the servo clockwise angle limit.
+ *
+ * @param servoId The servo id.
+ * @param cwAngleLimit The variable to store the clockwise angle limit.
+ * @return MX28_ERRBIT_NONE if succeeded, error code otherwise.
+ */
+ uint8_t GetCWAngleLimit(uint8_t servoId, uint16_t *cwAngleLimit);
+
+ /** Set the servo clockwise angle limit.
+ *
+ * @param servoId The servo id.
+ * @param cwAngleLimit The servo clockwise angle limit.
+ * @param isRegWrite If the command will be registered.
+ * @return MX28_ERRBIT_NONE if succeeded, error code otherwise.
+ */
+ uint8_t SetCWAngleLimit(uint8_t servoId, uint16_t cwAngleLimit, bool isRegWrite = false);
+
+ /** Get the servo counterclockwise angle limit.
+ *
+ * @param servoId The servo id.
+ * @param ccwAngleLimit The variable to store the counterclockwise angle limit.
+ * @return MX28_ERRBIT_NONE if succeeded, error code otherwise.
+ */
+ uint8_t GetCCWAngleLimit(uint8_t servoId, uint16_t *ccwAngleLimit);
+
+ /** Set the servo counterclockwise angle limit.
+ *
+ * @param servoId The servo id.
+ * @param ccwAngleLimit The servo counterclockwise angle limit.
+ * @param isRegWrite If the command will be registered.
+ * @return MX28_ERRBIT_NONE if succeeded, error code otherwise.
+ */
+ uint8_t SetCCWAngleLimit(uint8_t servoId, uint16_t ccwAngleLimit, bool isRegWrite = false);
+
+ /** Get the servo up temperature limit.
+ *
+ * @param servoId The servo id.
+ * @param highestTemperatureLimit The variable to store the highest temperature limit.
+ * @return MX28_ERRBIT_NONE if succeeded, error code otherwise.
+ */
+ uint8_t GetHighestTemperatureLimit(uint8_t servoId, uint8_t *highestTemperatureLimit);
+
+ /** Set the servo highest temperature limit.
+ *
+ * @param servoId The servo id.
+ * @param highestTemperatureLimit The servo highest temperature limit.
+ * @param isRegWrite If the command will be registered.
+ * @return MX28_ERRBIT_NONE if succeeded, error code otherwise.
+ */
+ uint8_t SetHighestTemperatureLimit(uint8_t servoId, uint8_t highestTemperatureLimit, bool isRegWrite = false);
+
+ /** Get the servo lowest voltage limit.
+ *
+ * @param servoId The servo id.
+ * @param lowestVoltageLimit The variable to store the lowest voltage limit.
+ * @return MX28_ERRBIT_NONE if succeeded, error code otherwise.
+ */
+ uint8_t GetLowestVoltageLimit(uint8_t servoId, uint8_t *lowestVoltageLimit);
+
+ /** Set the servo lowest voltage limit.
+ *
+ * @param servoId The servo id.
+ * @param lowestVoltageLimit The servo lowest voltage limit.
+ * @param isRegWrite If the command will be registered.
+ * @return MX28_ERRBIT_NONE if succeeded, error code otherwise.
+ */
+ uint8_t SetLowestVoltageLimit(uint8_t servoId, uint8_t lowestVoltageLimit, bool isRegWrite = false);
+
+ /** Get the servo highest voltage limit.
+ *
+ * @param servoId The servo id.
+ * @param highestVoltageLimit The variable to store the highest voltage limit.
+ * @return MX28_ERRBIT_NONE if succeeded, error code otherwise.
+ */
+ uint8_t GetHighestVoltageLimit(uint8_t servoId, uint8_t *highestVoltageLimit);
+
+ /** Set the servo highest voltage limit.
+ *
+ * @param servoId The servo id.
+ * @param highestVoltageLimit The servo highest voltage limit.
+ * @param isRegWrite If the command will be registered.
+ * @return MX28_ERRBIT_NONE if succeeded, error code otherwise.
+ */
+ uint8_t SetHighestVoltageLimit(uint8_t servoId, uint8_t highestVoltageLimit, bool isRegWrite = false);
+
+ /** Get the servo max torque.
+ *
+ * @param servoId The servo id.
+ * @param maxTorque The variable to store the max torque.
+ * @return MX28_ERRBIT_NONE if succeeded, error code otherwise.
+ */
+ uint8_t GetMaxTorque(uint8_t servoId, uint16_t *maxTorque);
+
+ /** Set the servo max torque.
+ *
+ * @param servoId The servo id.
+ * @param maxTorque The servo max torque.
+ * @param isRegWrite If the command will be registered.
+ * @return MX28_ERRBIT_NONE if succeeded, error code otherwise.
+ */
+ uint8_t SetMaxTorque(uint8_t servoId, uint16_t maxTorque, bool isRegWrite = false);
+
+ /** Get the servo status return level.
+ *
+ * @param servoId The servo id.
+ * @param statusReturnLevel The variable to store the status return level.
+ * @return MX28_ERRBIT_NONE if succeeded, error code otherwise.
+ */
+ uint8_t GetStatusReturnLevel(uint8_t servoId, uint8_t *statusReturnLevel);
+
+ /** Set the servo status return level.
+ *
+ * @param servoId The servo id.
+ * @param statusReturnLevel The servo status return level.
+ * @param isRegWrite If the command will be registered.
+ * @return MX28_ERRBIT_NONE if succeeded, error code otherwise.
+ */
+ uint8_t SetStatusReturnLevel(uint8_t servoId, uint8_t statusReturnLevel, bool isRegWrite = false);
+
+ /** Get the servo alarm LED.
+ *
+ * @param servoId The servo id.
+ * @param alarmLED The variable to store the alarm LED.
+ * @return MX28_ERRBIT_NONE if succeeded, error code otherwise.
+ */
+ uint8_t GetAlarmLED(uint8_t servoId, uint8_t *alarmLED);
+
+ /** Set the servo alarm LED.
+ *
+ * @param servoId The servo id.
+ * @param alarmLED The servo alarm LED.
+ * @param isRegWrite If the command will be registered.
+ * @return MX28_ERRBIT_NONE if succeeded, error code otherwise.
+ */
+ uint8_t SetAlarmLED(uint8_t servoId, uint8_t alarmLED, bool isRegWrite = false);
+
+ /** Get the servo alarm shutdown.
+ *
+ * @param servoId The servo id.
+ * @param alarmShutdown The variable to store the alarm shutdown.
+ * @return MX28_ERRBIT_NONE if succeeded, error code otherwise.
+ */
+ uint8_t GetAlarmShutdown(uint8_t servoId, uint8_t *alarmShutdown);
+
+ /** Set the servo alarm shutdown.
+ *
+ * @param servoId The servo id.
+ * @param alarmShutdown The servo alarm shutdown.
+ * @param isRegWrite If the command will be registered.
+ * @return MX28_ERRBIT_NONE if succeeded, error code otherwise.
+ */
+ uint8_t SetAlarmShutdown(uint8_t servoId, uint8_t alarmShutdown, bool isRegWrite = false);
+
+ /** Get the servo enable torque.
+ *
+ * @param servoId The servo id.
+ * @param enableTorque The variable to store the enable torque.
+ * @return MX28_ERRBIT_NONE if succeeded, error code otherwise.
+ */
+ uint8_t GetEnableTorque(uint8_t servoId, uint8_t *enableTorque);
+
+ /** Set the servo enable torque.
+ *
+ * @param servoId The servo id.
+ * @param enableTorque The servo enable torque.
+ * @param isRegWrite If the command will be registered.
+ * @return MX28_ERRBIT_NONE if succeeded, error code otherwise.
+ */
+ uint8_t SetEnableTorque(uint8_t servoId, uint8_t enableTorque, bool isRegWrite = false);
+
+ /** Get the servo enable LED.
+ *
+ * @param servoId The servo id.
+ * @param enableLED The variable to store the enable LED.
+ * @return MX28_ERRBIT_NONE if succeeded, error code otherwise.
+ */
+ uint8_t GetEnableLED(uint8_t servoId, uint8_t *enableLED);
+
+ /** Set the servo enable LED.
+ *
+ * @param servoId The servo id.
+ * @param enableLED The servo enable LED.
+ * @param isRegWrite If the command will be registered.
+ * @return MX28_ERRBIT_NONE if succeeded, error code otherwise.
+ */
+ uint8_t SetEnableLED(uint8_t servoId, uint8_t enableLED, bool isRegWrite = false);
+
+ /** Get the servo P gain.
+ *
+ * @param servoId The servo id.
+ * @param pGain The variable to store the P gain.
+ * @return MX28_ERRBIT_NONE if succeeded, error code otherwise.
+ */
+ uint8_t GetPGain(uint8_t servoId, uint8_t *pGain);
+
+ /** Set the servo P gain.
+ *
+ * @param servoId The servo id.
+ * @param pGain The servo P gain.
+ * @param isRegWrite If the command will be registered.
+ * @return MX28_ERRBIT_NONE if succeeded, error code otherwise.
+ */
+ uint8_t SetPGain(uint8_t servoId, uint8_t pGain, bool isRegWrite = false);
+
+ /** Get the servo I gain.
+ *
+ * @param servoId The servo id.
+ * @param iGain The variable to store the I gain.
+ * @return MX28_ERRBIT_NONE if succeeded, error code otherwise.
+ */
+ uint8_t GetIGain(uint8_t servoId, uint8_t *iGain);
+
+ /** Set the servo I gain.
+ *
+ * @param servoId The servo id.
+ * @param iGain The servo I gain.
+ * @param isRegWrite If the command will be registered.
+ * @return MX28_ERRBIT_NONE if succeeded, error code otherwise.
+ */
+ uint8_t SetIGain(uint8_t servoId, uint8_t iGain, bool isRegWrite = false);
+
+ /** Get the servo D gain.
+ *
+ * @param servoId The servo id.
+ * @param dGain The variable to store the D gain.
+ * @return MX28_ERRBIT_NONE if succeeded, error code otherwise.
+ */
+ uint8_t GetDGain(uint8_t servoId, uint8_t *dGain);
+
+ /** Set the servo D gain.
+ *
+ * @param servoId The servo id.
+ * @param dGain The servo D gain.
+ * @param isRegWrite If the command will be registered.
+ * @return MX28_ERRBIT_NONE if succeeded, error code otherwise.
+ */
+ uint8_t SetDGain(uint8_t servoId, uint8_t dGain, bool isRegWrite = false);
+
+ /** Get the servo goal position.
+ *
+ * @param servoId The servo id.
+ * @param goalPosition The variable to store the goal position.
+ * @return MX28_ERRBIT_NONE if succeeded, error code otherwise.
+ */
+ uint8_t GetGoalPosition(uint8_t servoId, uint16_t *goalPosition);
+
+ /** Set the servo goal position.
+ *
+ * @param servoId The servo id.
+ * @param goalPosition The servo goal position.
+ * @param isRegWrite If the command will be registered.
+ * @return MX28_ERRBIT_NONE if succeeded, error code otherwise.
+ */
+ uint8_t SetGoalPosition(uint8_t servoId, uint16_t goalPosition, bool isRegWrite = false);
+
+ /** Get the servo moving speed.
+ *
+ * @param servoId The servo id.
+ * @param movingSpeed The variable to store the moving speed.
+ * @return MX28_ERRBIT_NONE if succeeded, error code otherwise.
+ */
+ uint8_t GetMovingSpeed(uint8_t servoId, uint16_t *movingSpeed);
+
+ /** Set the servo moving speed.
+ *
+ * @param servoId The servo id.
+ * @param movingSpeed The servo moving speed.
+ * @param isRegWrite If the command will be registered.
+ * @return MX28_ERRBIT_NONE if succeeded, error code otherwise.
+ */
+ uint8_t SetMovingSpeed(uint8_t servoId, uint16_t movingSpeed, bool isRegWrite = false);
+
+ /** Get the servo torque limit.
+ * 0 to 1023 (0x3FF) is available, and the unit is about 0.1%.
+ * For example, if the value is 512, it is about 50%; that means only 50% of the maximum torque will be used.
+ * If the power is turned on, the value of Max Torque (Address 14, 15) is used as the initial value.
+ *
+ * @param servoId The servo id.
+ * @param torqueLimit The variable to store the torque limit.
+ * @return MX28_ERRBIT_NONE if succeeded, error code otherwise.
+ */
+ uint8_t GetTorqueLimit(uint8_t servoId, uint16_t *torqueLimit);
+
+ /** Set the servo torque limit.
+ *
+ * @param servoId The servo id.
+ * @param torqueLimit The servo torque limit.
+ * @param isRegWrite If the command will be registered.
+ * @return MX28_ERRBIT_NONE if succeeded, error code otherwise.
+ */
+ uint8_t SetTorqueLimit(uint8_t servoId, uint16_t torqueLimit, bool isRegWrite = false);
+
+ /** Get the servo present position.
+ * The range of the value is 0~4095 (0xFFF), and the unit is 0.088 degree.
+ *
+ * @param servoId The servo id.
+ * @param presentPosition The variable to store the present position.
+ * @return MX28_ERRBIT_NONE if succeeded, error code otherwise.
+ */
+ uint8_t GetPresentPosition(uint8_t servoId, uint16_t *presentPosition);
+
+ /** Get the servo present speed.
+ * 0~2047 (0x000~0X7FF) can be used.
+ * If a value is in the rage of 0~1023 then the motor rotates to the CCW direction.
+ * If a value is in the rage of 1024~2047 then the motor rotates to the CW direction.
+ * The 10th bit becomes the direction bit to control the direction; 0 and 1024 are equal.
+ * The value unit is about 0.11rpm.
+ * For example, if it is set to 300 then the motor is moving to the CCW direction at a rate of about 34.33rpm.
+ *
+ * @param servoId The servo id.
+ * @param presentSpeed The variable to store the present speed.
+ * @return MX28_ERRBIT_NONE if succeeded, error code otherwise.
+ */
+ uint8_t GetPresentSpeed(uint8_t servoId, uint16_t *presentSpeed);
+
+ /** Get the servo present load.
+ * The range of the value is 0~2047, and the unit is about 0.1%.
+ * If the value is 0~1023, it means the load works to the CCW direction.
+ * If the value is 1024~2047, it means the load works to the CW direction.
+ * That is, the 10th bit becomes the direction bit to control the direction, and 1024 is equal to 0.
+ * For example, the value is 512, it means the load is detected in the direction of CCW about 50% of the maximum torque.
+ *
+ * @param servoId The servo id.
+ * @param presentLoad The variable to store the present load.
+ * @return MX28_ERRBIT_NONE if succeeded, error code otherwise.
+ */
+ uint8_t GetPresentLoad(uint8_t servoId, uint16_t *presentLoad);
+
+ /** Get the servo present voltage.
+ * This value is 10 times larger than the actual voltage.
+ * For example, when 10V is supplied, the data value is 100 (0x64)
+ *
+ * @param servoId The servo id.
+ * @param presentVoltage The variable to store the present voltage.
+ * @return MX28_ERRBIT_NONE if succeeded, error code otherwise.
+ */
+ uint8_t GetPresentVoltage(uint8_t servoId, uint8_t *presentVoltage);
+
+ /** Get the servo present temperature.
+ * It is the internal temperature of Dynamixel in Celsius.
+ * Data value is identical to the actual temperature in Celsius.
+ * For example, if the data value is 85 (0x55), the current internal temperature is 85℃.
+ *
+ * @param servoId The servo id.
+ * @param presentTemperature The variable to store the present temperature.
+ * @return MX28_ERRBIT_NONE if succeeded, error code otherwise.
+ */
+ uint8_t GetPresentTemperature(uint8_t servoId, uint8_t *presentTemperature);
+
+ /** Get if there are commands transmitted by MX28_REG_WRITE.
+ * 0 There are no commands transmitted by REG_WRITE.
+ * 1 There are commands transmitted by REG_WRITE.
+ *
+ * @param servoId The servo id.
+ * @param isRegistered The variable to store if it is registered.
+ * @return MX28_ERRBIT_NONE if succeeded, error code otherwise.
+ */
+ uint8_t GetIsRegistered(uint8_t servoId, uint8_t *isRegistered);
+
+ /** Get if the servo is moving.
+ * 0 Goal position command execution is completed.
+ * 1 Goal position command execution is in progress.
+ *
+ * @param servoId The servo id.
+ * @param isMoving The variable to store if the servo is moving.
+ * @return MX28_ERRBIT_NONE if succeeded, error code otherwise.
+ */
+ uint8_t GetIsMoving(uint8_t servoId, uint8_t *isMoving);
+
+ /** Get if the servo EEPROM is locked.
+ * 0 EEPROM area can be modified.
+ * 1 EEPROM area cannot be modified.
+ *
+ * @param servoId The servo id.
+ * @param isLock The variable to store if the servo EEPROM is locked.
+ * @return MX28_ERRBIT_NONE if succeeded, error code otherwise.
+ */
+ uint8_t GetIsLocked(uint8_t servoId, uint8_t *isLocked);
+
+ /** Set if the servo EEPROM is locked.
+ *
+ * @param servoId The servo id.
+ * @param isLocked The variable to store if the servo EEPROM is locked.
+ * @param isRegWrite If the command will be registered.
+ * @return MX28_ERRBIT_NONE if succeeded, error code otherwise.
+ */
+ uint8_t SetIsLocked(uint8_t servoId, uint8_t isLocked, bool isRegWrite = false);
+
+ /** Get the servo punch.
+ * Can choose vales from 0x00 to 0x3FF.
+ *
+ * @param servoId The servo id.
+ * @param punch The variable to store the servo punch.
+ * @return MX28_ERRBIT_NONE if succeeded, error code otherwise.
+ */
+ uint8_t GetPunch(uint8_t servoId, uint16_t *punch);
+
+ /** Set the servo punch.
+ *
+ * @param servoId The servo id.
+ * @param punch The servo punch value.
+ * @param isRegWrite If the command will be registered.
+ * @return MX28_ERRBIT_NONE if succeeded, error code otherwise.
+ */
+ uint8_t SetPunch(uint8_t servoId, uint16_t punch, bool isRegWrite = false);
+
+ /** Ping the servo.
+ * No action. Used for obtaining a Status Packet.
+ *
+ * @param servoId The servo id.
+ * @return MX28_ERRBIT_NONE if succeeded, error code otherwise.
+ */
+ uint8_t Ping(uint8_t servoId);
+
+ /** Reset the servo.
+ * Changes the control table values of the Dynamixel actuator
+ * to the Factory Default Value settings
+ *
+ * @param servoId The servo id.
+ * @return MX28_ERRBIT_NONE if succeeded, error code otherwise.
+ */
+ uint8_t Reset(uint8_t servoId);
+
+ /** Trigger the servo.
+ * Triggers the action registered by the REG_WRITE instruction
+ *
+ * @param servoId The servo id.
+ * @return MX28_ERRBIT_NONE if succeeded, error code otherwise.
+ */
+ uint8_t Action(uint8_t servoId);
+
+ /** Write data to multiple servos.
+ * Used for controlling many Dynamixel actuators at the same time.
+ *
+ * @param data The data array.
+ * @param data The length of the array.
+ * @return MX28_ERRBIT_NONE if succeeded, error code otherwise.
+ */
+ uint8_t SyncWrite(uint8_t *data, uint8_t length);
+
+ /** Create an MX28 servo object connected to the specified serial half duplex pins,
+ * with the specified baudrate.
+ *
+ * @param tx Send pin.
+ * @param rx Receive pin.
+ * @param baudrate The bus speed.
+ */
+ MX28(PinName tx, PinName rx, int baudRate);
+
+ /** Destroy an MX28 servo object
+ */
+ ~MX28();
+};
+
+#endif // MX28_H
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Protocol.h Sun Sep 09 22:09:00 2012 +0000
@@ -0,0 +1,124 @@
+/* Dynamixel MX28 servo library
+ * Copyright (c) 2012 Georgios Petrou, MIT License
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy of this software
+ * and associated documentation files (the "Software"), to deal in the Software without restriction,
+ * including without limitation the rights to use, copy, modify, merge, publish, distribute,
+ * sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all copies or
+ * substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
+ * BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
+ * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
+
+#ifndef PROTOCOL_H
+#define PROTOCOL_H
+
+#include "mbed.h"
+
+#define MX28_BUFFER_SIZE 0x8F
+
+#define MX28_PROTOCOL_COMMAND_RESPONSE_TIMEOUT_MS 200
+
+#define MX28_PROTOCOL_HEADER_0 0xFF
+#define MX28_PROTOCOL_HEADER_1 0xFF
+
+#define MX28_PROTOCOL_BROADCAST_ID 0xFE
+
+// EEPROM Area
+#define MX28_MODEL_NUMBER_L 0x00
+#define MX28_MODEL_NUMBER_H 0x01
+#define MX28_VERSION 0x02
+#define MX28_ID 0x03
+#define MX28_BAUD_RATE 0x04
+#define MX28_RETURN_DELAY_TIME 0x05
+#define MX28_CW_ANGLE_LIMIT_L 0x06
+#define MX28_CW_ANGLE_LIMIT_H 0x07
+#define MX28_CCW_ANGLE_LIMIT_L 0x08
+#define MX28_CCW_ANGLE_LIMIT_H 0x09
+#define MX28_UP_LIMIT_TEMPERATURE 0x0B
+#define MX28_DOWN_LIMIT_VOLTAGE 0x0C
+#define MX28_UP_LIMIT_VOLTAGE 0x0D
+#define MX28_MAX_TORQUE_L 0x0E
+#define MX28_MAX_TORQUE_H 0x0F
+#define MX28_STATUS_RETURN_LEVEL 0x10
+#define MX28_ALARM_LED 0x11
+#define MX28_ALARM_SHUTDOWN 0x12
+
+// RAM Area
+#define MX28_TORQUE_ENABLE 0x18
+#define MX28_LED_ENABLE 0x19
+#define MX28_D_GAIN 0x1A
+#define MX28_I_GAIN 0x1B
+#define MX28_P_GAIN 0x1C
+#define MX28_GOAL_POSITION_L 0x1E
+#define MX28_GOAL_POSITION_H 0x1F
+#define MX28_MOVING_SPEED_L 0x20
+#define MX28_MOVING_SPEED_H 0x21
+#define MX28_TORQUE_LIMIT_L 0x22
+#define MX28_TORQUE_LIMIT_H 0x23
+#define MX28_PRESENT_POSITION_L 0x24
+#define MX28_PRESENT_POSITION_H 0x25
+#define MX28_PRESENT_SPEED_L 0x26
+#define MX28_PRESENT_SPEED_H 0x27
+#define MX28_PRESENT_LOAD_L 0x28
+#define MX28_PRESENT_LOAD_H 0x29
+#define MX28_PRESENT_VOLTAGE 0x2A
+#define MX28_PRESENT_TEMPERATURE 0x2B
+#define MX28_REGISTERED_INSTRUCTION 0x2C
+#define MX28_MOVING 0x3E
+#define MX28_LOCK 0x2F
+#define MX28_PUNCH_L 0x30
+#define MX28_PUNCH_H 0x31
+
+// Instruction set
+#define MX28_PING 0x01
+#define MX28_READ_DATA 0x02
+#define MX28_WRITE_DATA 0x03
+#define MX28_REG_WRITE 0x04
+#define MX28_ACTION 0x05
+#define MX28_RESET 0x06
+#define MX28_SYNC_WRITE 0x83
+
+// Errors
+#define MX28_ERRBIT_NONE 0x00
+#define MX28_ERRBIT_VOLTAGE 0x01
+#define MX28_ERRBIT_ANGLE 0x02
+#define MX28_ERRBIT_OVERHEAT 0x04
+#define MX28_ERRBIT_RANGE 0x08
+#define MX28_ERRBIT_CHECKSUM 0x10
+#define MX28_ERRBIT_OVERLOAD 0x20
+#define MX28_ERRBIT_INSTRUCTION 0x40
+
+// Extra errors
+#define MX28_ERRBIT_WRITE_TIMEOUT 0xFD
+#define MX28_ERRBIT_READ_TIMEOUT 0xFE
+#define MX28_ERRBIT_MASTER_CHECKSUM 0xFF
+
+struct MX28_PROTOCOL_PACKET
+{
+ uint8_t servoId;
+ uint8_t length;
+ uint8_t instructionErrorId;
+ uint8_t parameter[MX28_BUFFER_SIZE];
+ uint8_t checkSum;
+};
+
+enum MX28_PROTOCOL_ENCODER_DECODER_STATE
+{
+ WAIT_ON_HEADER_0,
+ WAIT_ON_HEADER_1,
+ WAIT_ON_ADDRESSED_NODE_ID,
+ WAIT_ON_LENGTH,
+ WAIT_ON_INSTRUCTION_ERROR_ID,
+ WAIT_ON_PARAMETER,
+ WAIT_ON_CHECK_SUM
+};
+
+#endif // PROTOCOL_H
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Utilities.cpp Sun Sep 09 22:09:00 2012 +0000
@@ -0,0 +1,75 @@
+/* Copyright (c) 2012 Georgios Petrou, MIT License
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy of this software
+ * and associated documentation files (the "Software"), to deal in the Software without restriction,
+ * including without limitation the rights to use, copy, modify, merge, publish, distribute,
+ * sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all copies or
+ * substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
+ * BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
+ * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
+
+#include "Utilities.h"
+
+uint8_t Utilities::GetCheckSum(const uint8_t *data, uint8_t length)
+{
+ uint8_t checkSum = 0;
+
+ for(int i = 0; i < length; i++)
+ checkSum += data[i];
+
+ checkSum = ~checkSum;
+
+ return checkSum;
+}
+
+void Utilities::ConvertUInt16ToUInt8Array(uint16_t value, uint8_t* data)
+{
+ data[0] = value;
+ data[1] = value >> 8;
+}
+
+void Utilities::ConvertInt16ToUInt8Array(int16_t value, uint8_t* data)
+{
+ data[0] = value;
+ data[1] = value >> 8;
+}
+
+void Utilities::ConvertUInt32ToUInt8Array(uint32_t value, uint8_t* data)
+{
+ data[0] = value;
+ data[1] = value >> 8;
+ data[2] = value >> 16;
+ data[3] = value >> 24;
+}
+
+void Utilities::ConvertInt32ToUInt8Array(int32_t value, uint8_t* data)
+{
+ data[0] = value;
+ data[1] = value >> 8;
+ data[2] = value >> 16;
+ data[3] = value >> 24;
+}
+
+uint16_t Utilities::ConvertUInt8ArrayToUInt16(uint8_t* data)
+{
+ return (((uint16_t)data[1]) << 8) | ((uint16_t)data[0]);
+}
+
+int16_t Utilities::ConvertUInt8ArrayToInt16(uint8_t* data)
+{
+ return (((int16_t)data[1]) << 8) | ((int16_t)data[0]);
+}
+
+int32_t Utilities::ConvertUInt8ArrayToInt32(uint8_t* data)
+{
+ return (((int32_t)data[3]) << 24) | (((int32_t)data[2]) << 16) | (((int32_t)data[1]) << 8) | ((int32_t)data[0]);
+}
+
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Utilities.h Sun Sep 09 22:09:00 2012 +0000
@@ -0,0 +1,86 @@
+/* Copyright (c) 2012 Georgios Petrou, MIT License
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy of this software
+ * and associated documentation files (the "Software"), to deal in the Software without restriction,
+ * including without limitation the rights to use, copy, modify, merge, publish, distribute,
+ * sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all copies or
+ * substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
+ * BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
+ * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
+
+#ifndef UTILITIES_H
+#define UTILITIES_H
+
+#include "mbed.h"
+
+class Utilities
+{
+ public:
+ /** Get the checksum for a packet.
+ *
+ * @param data The array of bytes.
+ * @param length The size of the array.
+ * @return checksum
+ */
+ static uint8_t GetCheckSum(const uint8_t *data, uint8_t length);
+
+ /** Convert a UInt16 number to array of chars.
+ *
+ * @param value The number value.
+ * @param data The array to store the conversion.
+ */
+ static void ConvertUInt16ToUInt8Array(uint16_t value, uint8_t* data);
+
+ /** Convert an Int16 number to array of chars.
+ *
+ * @param value The number value.
+ * @param data The array to store the conversion.
+ */
+ static void ConvertInt16ToUInt8Array(int16_t value, uint8_t* data);
+
+ /** Convert a UInt32 number to array of chars.
+ *
+ * @param value The number value.
+ * @param data The array to store the conversion.
+ */
+ static void ConvertUInt32ToUInt8Array(uint32_t value, uint8_t* data);
+
+ /** Convert an Int32 number to array of chars.
+ *
+ * @param value The number value.
+ * @param data The array to store the conversion.
+ */
+ static void ConvertInt32ToUInt8Array(int32_t value, uint8_t* data);
+
+ /** Convert an array of char to UInt16.
+ *
+ * @param data The array containing the chars.
+ * @return UInt16 converted number.
+ */
+ static uint16_t ConvertUInt8ArrayToUInt16(uint8_t* data);
+
+ /** Convert an array of char to Int16.
+ *
+ * @param data The array containing the chars.
+ * @return Int16 converted number.
+ */
+ static int16_t ConvertUInt8ArrayToInt16(uint8_t* data);
+
+ /** Convert an array of char to Int32.
+ *
+ * @param data The array containing the chars.
+ * @return Int32 number.
+ */
+ static int32_t ConvertUInt8ArrayToInt32(uint8_t* data);
+};
+
+#endif // UTILITIES_H
+