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

Dependents:   IDcheck

Fork of MX28 by Georgios Petrou

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

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

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

Revision:
1:5f537df9dca8
Parent:
0:ea5b951002cf
Child:
2:85216442d3ef
diff -r ea5b951002cf -r 5f537df9dca8 MX28.h
--- 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