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 1:5f537df9dca8, committed 2012-09-12
- Comitter:
- GIPetrou
- Date:
- Wed Sep 12 15:59:54 2012 +0000
- Parent:
- 0:ea5b951002cf
- Child:
- 2:85216442d3ef
- Commit message:
- First commit of library
Changed in this revision
--- a/MX28.cpp Sun Sep 09 22:09:00 2012 +0000
+++ b/MX28.cpp Wed Sep 12 15:59:54 2012 +0000
@@ -1,1526 +1,1510 @@
-/* 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;
-}
+/* 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_SERVO_ID;
+
+ break;
+ }
+ case WAIT_ON_SERVO_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_SERVO_ID;
+
+ break;
+ }
+ case WAIT_ON_SERVO_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);
+}
+
+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;
+}
--- a/MX28.h Sun Sep 09 22:09:00 2012 +0000
+++ b/MX28.h Wed Sep 12 15:59:54 2012 +0000
@@ -1,714 +1,778 @@
-/* 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();
-};
-
+/* 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);
+*
+* uint8_t servo1Id = 0x01;
+* uint8_t servo2Id = 0x02;
+* uint8_t servo3Id = 0x03;
+*
+* uint16_t servo1GoalPosition = 0x0800;
+* uint16_t servo2GoalPosition = 0x0800;
+* uint16_t servo3GoalPosition = 0x0800;
+*
+* MX28_PROTOCOL_PACKET packet;
+* packet.servoId = MX28_PROTOCOL_BROADCAST_ID;
+* // (Data length + 1) * Number of servos + 4
+* packet.length = (2+ 1) * 3 + 4;
+* packet.instructionErrorId = MX28_SYNC_WRITE;
+* packet.parameter[0] = MX28_GOAL_POSITION_L;
+* packet.parameter[1] = 0x06;
+* packet.parameter[2] = servo1Id;
+* Utilities::ConvertUInt16ToUInt8Array(servo1GoalPosition, (uint8_t*)&(packet.parameter[3]));
+* packet.parameter[9] = servo2Id;
+* Utilities::ConvertUInt16ToUInt8Array(servo2GoalPosition, (uint8_t*)&(packet.parameter[10]));
+* packet.parameter[16] = servo3Id;
+* Utilities::ConvertUInt16ToUInt8Array(servo3GoalPosition, (uint8_t*)&(packet.parameter[17]));
+*
+* pc.printf("Set servos goal positions: %hu %hu %hu\r\n", servo1GoalPosition, servo2GoalPosition, servo3GoalPosition);
+*
+* mx28.CommunicatePacket(&packet);
+*
+* packet.servoId = servoId;
+* packet.length = 4;
+* packet.instructionErrorId = MX28_READ_DATA;
+* packet.parameter[0] = MX28_PRESENT_POSITION_L;
+* packet.parameter[1] = 0x08;
+*
+* mx28.CommunicatePacket(&packet);
+*
+* presentPosition = Utilities::ConvertUInt8ArrayToUInt16(packet.parameter);
+* presentSpeed = Utilities::ConvertUInt8ArrayToUInt16((uint8_t*)&(packet.parameter[2]));
+* presentLoad = Utilities::ConvertUInt8ArrayToUInt16((uint8_t*)&(packet.parameter[4]));
+* presentVoltage = packet.parameter[6];
+* presentTemperature = packet.parameter[7];
+*
+* pc.printf("Present position: %hu\r\n", presentPosition);
+* pc.printf("Present speed: %hu\r\n", presentSpeed);
+* pc.printf("Present load: %hu\r\n", presentLoad);
+* pc.printf("Present voltage: 0x%02X\r\n", presentVoltage);
+* pc.printf("Present temperature: 0x%02X\r\n", presentTemperature);
+*
+* uint8_t status = mx28.GetModelNumber(servoId, &modelNumber);
+*
+* if(status == MX28_ERRBIT_WRITE_TIMEOUT)
+* pc.printf("Error: Write timeout\r\n");
+* else if(status == MX28_ERRBIT_READ_TIMEOUT)
+* pc.printf("Error: Read timeout\r\n");
+* else if(status == MX28_ERRBIT_MASTER_CHECKSUM)
+* pc.printf("Error: Master checksum error\r\n");
+* else
+* {
+* if(status & MX28_ERRBIT_VOLTAGE)
+* pc.printf("Error: Input voltage error\r\n");
+* if(status & MX28_ERRBIT_ANGLE)
+* pc.printf("Error: Angle limit error\r\n");
+* if(status & MX28_ERRBIT_OVERHEAT)
+* pc.printf("Error: Overheat error\r\n");
+* if(status & MX28_ERRBIT_RANGE)
+* pc.printf("Error: Out of range error\r\n");
+* if(status & MX28_ERRBIT_CHECKSUM)
+* pc.printf("Error: Checksum error\r\n");
+* if(status & MX28_ERRBIT_OVERLOAD)
+* pc.printf("Error: Overload error\r\n");
+* if(status & MX28_ERRBIT_INSTRUCTION)
+* pc.printf("Error: Instruction code error\r\n");
+* }
+*
+* 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);
+
+ /** 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
--- a/Protocol.h Sun Sep 09 22:09:00 2012 +0000
+++ b/Protocol.h Wed Sep 12 15:59:54 2012 +0000
@@ -114,7 +114,7 @@
{
WAIT_ON_HEADER_0,
WAIT_ON_HEADER_1,
- WAIT_ON_ADDRESSED_NODE_ID,
+ WAIT_ON_SERVO_ID,
WAIT_ON_LENGTH,
WAIT_ON_INSTRUCTION_ERROR_ID,
WAIT_ON_PARAMETER,
--- a/Utilities.h Sun Sep 09 22:09:00 2012 +0000
+++ b/Utilities.h Wed Sep 12 15:59:54 2012 +0000
@@ -21,6 +21,8 @@
#include "mbed.h"
+/** MX28 utility functions class
+*/
class Utilities
{
public: