XL320_Driver.h

00001 /*******************************************************************************
00002 This library was developed at ECAM Lyon by Alexandre Simon
00003 
00004 This code is inspired from an Mbed library for Dynamixel XL-320 developed by
00005 Muhammad Fajar Adityo.
00006 
00007 The XL-320 motor chain needs to be powered externally using for instance the
00008 card "Poppy PIXL" that can be found at https://github.com/poppy-project/pixl
00009 This board powers up your motors and provides a conversion from the 5V TTL half
00010 duplex bus of Dynamixel motors from the 3.3V UART ports of the Mbed board
00011 without echo of outcoming serial frames from Mbed microcontroller.
00012 
00013 This library is based on the Dynamixel XL-320 data sheet available at
00014 http://support.robotis.com/en/product/actuator/dynamixel_x/xl_series/xl-320.htm
00015 
00016 This library is based on the Dynamixel communication protocol available at
00017 http://support.robotis.com/en/product/actuator/dynamixel_pro/communication/
00018 instruction_status_packet.htm
00019 *******************************************************************************/
00020 
00021 #ifndef XL320_H
00022 #define XL320_H
00023 
00024 #include "mbed.h"
00025 
00026 static Serial pc(USBTX,USBRX);
00027 
00028 ////////// SHIFT BYTES ///////////
00029 #define SHIFT_TO_LSB(w)         (uint8_t((uint16_t(w)) & 0x00ff))
00030 #define SHIFT_TO_MSB(w)         (uint8_t((uint16_t(w) >> 8) & 0x00ff))
00031 
00032 ////////// EEPROM Address ////////      //Access    Initial Value
00033 #define MODEL_NUMBER            0x00    //R         350     -
00034 #define VERSION                 0x02    //R         -       -
00035 #define XL_ID                   0x03    //RW        1       0 ~ 252(0xFC) Broadcast = 0xFE
00036 #define BAUD_RATE               0x04    //RW        3       0 : 9600, 1 : 57600, 2 : 115200, 3 : 1Mbps
00037 #define RETURN_DELAY_TIME       0x05    //RW        250     0 ~ 254
00038 #define CW_ANGLE_LIMIT          0x06    //RW        0       0 ~ 1023
00039 #define CCW_ANGLE_LIMIT         0x08    //RW        1023    0 ~ 1023
00040 #define SYSTEM_DATA2            0x0A
00041 #define CONTROL_MODE            0x0B    //RW        2       1 : Wheel mode (spin infinitely), 2 : Join mode
00042 #define LIMIT_TEMPERATURE       0x0C    //RW        65      0 ~ 150
00043 #define LOWER_LIMIT_VOLTAGE     0x0D    //RW        60      50 ~ 250. 10 = 1V
00044 #define UPPER_LIMIT_VOLTAGE     0x0E    //RW        90      50 ~ 250. 10 = 1V
00045 #define MAX_TORQUE              0x0F    //RW        1023    0 ~ 1023
00046 #define RETURN_LEVEL            0x11    //RW        2       0 : No return excepted Ping, 1 : Return only for READ command, 2 : Return for all commands
00047 #define ALARM_SHUTDOWN          0x12    //RW        3       0 ~ 7
00048 
00049 ////////// RAM Address ///////////
00050 #define TORQUE_ENABLE           0X18    //RW        0       0 ~ 1
00051 #define LED                     0x19    //RW        0       0 ~ 7. 0 : off, 1 : red, 2 : green...
00052 #define D_GAIN                  0x1B    //RW        0       0 ~ 254
00053 #define I_GAIN                  0x1C    //RW        0       0 ~ 254
00054 #define P_GAIN                  0x1D    //RW        32      0 ~ 254
00055 #define GOAL_POSITION           0x1E    //RW        -       0 ~ 1023. 1 unit = 0.29 degree
00056 #define GOAL_SPEED              0x20    //RW        -       0 ~ 2047
00057 #define GOAL_TORQUE             0x23    //RW        -       0 ~ 1023
00058 #define PRESENT_POSITION        0x25    //R         -       -
00059 #define PRESENT_SPEED           0x27    //R         -       -
00060 #define PRESENT_LOAD            0x29    //R         -       -
00061 #define PRESENT_VOLTAGE         0x2D    //R         -       -
00062 #define PRESENT_TEMPERATURE     0x2E    //R         -       -
00063 #define REGISTERED_INSTRUCTION  0x2F    //R         0       -
00064 #define MOVING                  0x31    //R         0       -
00065 #define HARDWARE_ERROR_STATUS   0x32    //R         0       -
00066 #define PUNCH                   0x33    //RW        32      0 ~ 1023
00067 
00068 ////////// INSTRUCTION ///////////
00069 #define PING                    0x01
00070 #define READ_DATA               0x02
00071 #define WRITE_DATA              0x03
00072 #define REG_WRITE               0x04
00073 #define ACTION                  0x05
00074 #define FACTORY_RESET           0x06
00075 #define REBOOT                  0x08
00076 #define STATUS                  0x55
00077 #define SYNC_READ               0x82
00078 #define SYNC_WRITE              0x83
00079 #define BULK_READ               0x92
00080 #define BULK_WRITE              0x93
00081 
00082 ////////// ERROR ///////////
00083 #define RESULT_FAIL             0x01
00084 #define INS_ERR                 0x02
00085 #define CRC_ERR                 0x03
00086 #define DATA_RANGE_ERR          0x04
00087 #define DATA_LENGTH_ERR         0x05
00088 #define DATA_LIMIT_ERR          0x06
00089 #define ACCESS_ERR              0x07
00090 
00091 ////////// RESET ///////////
00092 #define RESET_ALL               0xFF
00093 #define RESET_EXP_ID            0x01
00094 #define RESET_EXP_ID_BR         0x02
00095 
00096 class XL320
00097 {
00098 public:
00099     XL320(uint32_t baud, PinName tx, PinName rx);
00100     ~XL320();
00101 
00102     ////// EEPROM ACCESS METHODS //////
00103 
00104     /***** XL320 Network Parameter *****/
00105     void SetID(uint8_t id, uint8_t newId);
00106     uint8_t GetID(uint8_t id);
00107     void SetBaudRate(uint8_t id, uint8_t baudrate);
00108     uint8_t GetBaudRate(uint8_t id);
00109     void SetRetDelTime(uint8_t id, uint8_t time);
00110     uint8_t GetRetDelTime(uint8_t id);
00111 
00112     /***** XL320 Motor Setting *****/
00113     void SetCWAngLim(uint8_t id, uint16_t angle);
00114     uint16_t GetCWAngLim(uint8_t id);
00115     void SetCCWAngLim(uint8_t id, uint16_t angle);
00116     uint16_t GetCCWAngLim(uint8_t id);
00117     void SetContMode(uint8_t id, uint8_t mode);
00118     uint8_t GetContMode(uint8_t id);
00119     void SetTempLim(uint8_t id, uint8_t temp);
00120     uint8_t GetTempLim(uint8_t id);
00121     void SetLowVoltLim(uint8_t id, uint8_t volt);
00122     uint8_t GetLowVoltLim(uint8_t id);
00123     void SetUpVoltLim(uint8_t id, uint8_t volt);
00124     uint8_t GetUpVoltLim(uint8_t id);
00125     void SetMaxTorq(uint8_t id, uint16_t torque);
00126     uint16_t GetMaxTorq(uint8_t id);
00127     void SetRetLev(uint8_t id, uint8_t level);
00128     uint8_t GetRetLev(uint8_t id);
00129     void SetAlarmShut(uint8_t id, uint8_t alarm);
00130     uint8_t GetAlarmShut(uint8_t id);
00131 
00132     ////// RAM ACCESS METHODS //////
00133 
00134     /***** XL320 On/Off *****/
00135     void SetTorqueEn(uint8_t id, uint8_t enable);
00136     uint8_t GetTorqueEn(uint8_t id);
00137     void TurnOnLED(uint8_t id, uint8_t led);
00138     uint8_t GetStatusLED(uint8_t id);
00139 
00140     /***** XL320 Motor Control *****/
00141     void SetDGain(uint8_t id, uint8_t d_cons);
00142     uint8_t GetDGain(uint8_t id);
00143     void SetIGain(uint8_t id, uint8_t i_cons);
00144     uint8_t GetIGain(uint8_t id);
00145     void SetPGain(uint8_t id, uint8_t p_cons);
00146     uint8_t GetPGain(uint8_t id);
00147     void SetGoalPos(uint8_t id, uint16_t position);
00148     uint16_t GetGoalPos(uint8_t id);
00149     void SetGoalVel(uint8_t id, uint16_t velocity);
00150     uint16_t GetGoalVel(uint8_t id);
00151     void SetGoalTorq(uint8_t id, uint16_t torque);
00152     uint16_t GetGoalTorq(uint8_t id);
00153 
00154     /***** XL320 Feedback *****/
00155     uint16_t GetPresentPos(uint8_t id);
00156     uint16_t GetPresentSpeed(uint8_t id);
00157     uint16_t GetPresentLoad(uint8_t id);
00158     uint8_t GetPresentVolt(uint8_t id);
00159     uint8_t GetPresentTemp(uint8_t id);
00160 
00161     /***** XL320 Status *****/
00162     uint8_t GetRegInst(uint8_t id);
00163     uint8_t GetMoving(uint8_t id);
00164     uint8_t GetHWErr(uint8_t id);
00165     void SetPunch(uint8_t id, uint16_t punch);
00166     uint16_t GetPunch(uint8_t id);
00167 
00168     /***** XL320 Instruction Method *****/
00169     void FactoryReset(uint8_t id, uint8_t option);
00170     void Ping(uint8_t id);
00171 
00172 private:
00173     /***** Generic functions *****/
00174     void SetSomething(uint8_t id, uint16_t address, uint8_t param[], uint8_t paramLength);
00175     uint8_t GetSomething(uint8_t id, uint16_t address);
00176     uint16_t GetSomethingElse(uint8_t id, uint16_t address);
00177 
00178     /***** XL320 Instruction Method *****/
00179     void iRead(uint8_t id, uint8_t length, uint16_t address);
00180     void iWrite(uint8_t id, uint16_t address, uint8_t param[], uint8_t paramLength);
00181 
00182     /***** Dynamixel Protocol 2.0 Methods *****/
00183     void sendIPacket();
00184     void getRPacket();
00185 
00186     /***** Calculating CRC Method *****/
00187     unsigned short update_crc(unsigned short crc_accum, unsigned char *data_blk_ptr, unsigned short data_blk_size);
00188 
00189     /***** Variables *****/
00190     uint32_t baudrate;      //Baudrate
00191     uint8_t iPacket[20];    //Instruction Packet
00192     uint8_t iPacketLength;  //Instruction Packet Length
00193     uint8_t rPacket[20];    //Return Packet
00194     uint8_t rPacketLength;  //Return Packet Length
00195 
00196     Serial sXL320;          //Serial Line for XL320
00197 };
00198 
00199 #endif