Alexandre Simon / Mbed 2 deprecated Poppy_Ergo_Jr

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

Poppy_Arm/XL320_Driver/XL320_Driver.h@0:6e72b34b96c5, 2018-06-15 (annotated)

Committer:
dev_num_ecam
Date:
Fri Jun 15 12:25:29 2018 +0000
Revision:
0:6e72b34b96c5
V1.0 : first publishing

Who changed what in which revision?

UserRevisionLine numberNew contents of line
dev_num_ecam 0:6e72b34b96c5 1 /*******************************************************************************
dev_num_ecam 0:6e72b34b96c5 2 This library was developed at ECAM Lyon by Alexandre Simon
dev_num_ecam 0:6e72b34b96c5 3
dev_num_ecam 0:6e72b34b96c5 4 This code is inspired from an Mbed library for Dynamixel XL-320 developed by
dev_num_ecam 0:6e72b34b96c5 5 Muhammad Fajar Adityo.
dev_num_ecam 0:6e72b34b96c5 6
dev_num_ecam 0:6e72b34b96c5 7 The XL-320 motor chain needs to be powered externally using for instance the
dev_num_ecam 0:6e72b34b96c5 8 card "Poppy PIXL" that can be found at https://github.com/poppy-project/pixl
dev_num_ecam 0:6e72b34b96c5 9 This board powers up your motors and provides a conversion from the 5V TTL half
dev_num_ecam 0:6e72b34b96c5 10 duplex bus of Dynamixel motors from the 3.3V UART ports of the Mbed board
dev_num_ecam 0:6e72b34b96c5 11 without echo of outcoming serial frames from Mbed microcontroller.
dev_num_ecam 0:6e72b34b96c5 12
dev_num_ecam 0:6e72b34b96c5 13 This library is based on the Dynamixel XL-320 data sheet available at
dev_num_ecam 0:6e72b34b96c5 14 http://support.robotis.com/en/product/actuator/dynamixel_x/xl_series/xl-320.htm
dev_num_ecam 0:6e72b34b96c5 15
dev_num_ecam 0:6e72b34b96c5 16 This library is based on the Dynamixel communication protocol available at
dev_num_ecam 0:6e72b34b96c5 17 http://support.robotis.com/en/product/actuator/dynamixel_pro/communication/
dev_num_ecam 0:6e72b34b96c5 18 instruction_status_packet.htm
dev_num_ecam 0:6e72b34b96c5 19 *******************************************************************************/
dev_num_ecam 0:6e72b34b96c5 20
dev_num_ecam 0:6e72b34b96c5 21 #ifndef XL320_H
dev_num_ecam 0:6e72b34b96c5 22 #define XL320_H
dev_num_ecam 0:6e72b34b96c5 23
dev_num_ecam 0:6e72b34b96c5 24 #include "mbed.h"
dev_num_ecam 0:6e72b34b96c5 25
dev_num_ecam 0:6e72b34b96c5 26 static Serial pc(USBTX,USBRX);
dev_num_ecam 0:6e72b34b96c5 27
dev_num_ecam 0:6e72b34b96c5 28 ////////// SHIFT BYTES ///////////
dev_num_ecam 0:6e72b34b96c5 29 #define SHIFT_TO_LSB(w) (uint8_t((uint16_t(w)) & 0x00ff))
dev_num_ecam 0:6e72b34b96c5 30 #define SHIFT_TO_MSB(w) (uint8_t((uint16_t(w) >> 8) & 0x00ff))
dev_num_ecam 0:6e72b34b96c5 31
dev_num_ecam 0:6e72b34b96c5 32 ////////// EEPROM Address //////// //Access Initial Value
dev_num_ecam 0:6e72b34b96c5 33 #define MODEL_NUMBER 0x00 //R 350 -
dev_num_ecam 0:6e72b34b96c5 34 #define VERSION 0x02 //R - -
dev_num_ecam 0:6e72b34b96c5 35 #define XL_ID 0x03 //RW 1 0 ~ 252(0xFC) Broadcast = 0xFE
dev_num_ecam 0:6e72b34b96c5 36 #define BAUD_RATE 0x04 //RW 3 0 : 9600, 1 : 57600, 2 : 115200, 3 : 1Mbps
dev_num_ecam 0:6e72b34b96c5 37 #define RETURN_DELAY_TIME 0x05 //RW 250 0 ~ 254
dev_num_ecam 0:6e72b34b96c5 38 #define CW_ANGLE_LIMIT 0x06 //RW 0 0 ~ 1023
dev_num_ecam 0:6e72b34b96c5 39 #define CCW_ANGLE_LIMIT 0x08 //RW 1023 0 ~ 1023
dev_num_ecam 0:6e72b34b96c5 40 #define SYSTEM_DATA2 0x0A
dev_num_ecam 0:6e72b34b96c5 41 #define CONTROL_MODE 0x0B //RW 2 1 : Wheel mode (spin infinitely), 2 : Join mode
dev_num_ecam 0:6e72b34b96c5 42 #define LIMIT_TEMPERATURE 0x0C //RW 65 0 ~ 150
dev_num_ecam 0:6e72b34b96c5 43 #define LOWER_LIMIT_VOLTAGE 0x0D //RW 60 50 ~ 250. 10 = 1V
dev_num_ecam 0:6e72b34b96c5 44 #define UPPER_LIMIT_VOLTAGE 0x0E //RW 90 50 ~ 250. 10 = 1V
dev_num_ecam 0:6e72b34b96c5 45 #define MAX_TORQUE 0x0F //RW 1023 0 ~ 1023
dev_num_ecam 0:6e72b34b96c5 46 #define RETURN_LEVEL 0x11 //RW 2 0 : No return excepted Ping, 1 : Return only for READ command, 2 : Return for all commands
dev_num_ecam 0:6e72b34b96c5 47 #define ALARM_SHUTDOWN 0x12 //RW 3 0 ~ 7
dev_num_ecam 0:6e72b34b96c5 48
dev_num_ecam 0:6e72b34b96c5 49 ////////// RAM Address ///////////
dev_num_ecam 0:6e72b34b96c5 50 #define TORQUE_ENABLE 0X18 //RW 0 0 ~ 1
dev_num_ecam 0:6e72b34b96c5 51 #define LED 0x19 //RW 0 0 ~ 7. 0 : off, 1 : red, 2 : green...
dev_num_ecam 0:6e72b34b96c5 52 #define D_GAIN 0x1B //RW 0 0 ~ 254
dev_num_ecam 0:6e72b34b96c5 53 #define I_GAIN 0x1C //RW 0 0 ~ 254
dev_num_ecam 0:6e72b34b96c5 54 #define P_GAIN 0x1D //RW 32 0 ~ 254
dev_num_ecam 0:6e72b34b96c5 55 #define GOAL_POSITION 0x1E //RW - 0 ~ 1023. 1 unit = 0.29 degree
dev_num_ecam 0:6e72b34b96c5 56 #define GOAL_SPEED 0x20 //RW - 0 ~ 2047
dev_num_ecam 0:6e72b34b96c5 57 #define GOAL_TORQUE 0x23 //RW - 0 ~ 1023
dev_num_ecam 0:6e72b34b96c5 58 #define PRESENT_POSITION 0x25 //R - -
dev_num_ecam 0:6e72b34b96c5 59 #define PRESENT_SPEED 0x27 //R - -
dev_num_ecam 0:6e72b34b96c5 60 #define PRESENT_LOAD 0x29 //R - -
dev_num_ecam 0:6e72b34b96c5 61 #define PRESENT_VOLTAGE 0x2D //R - -
dev_num_ecam 0:6e72b34b96c5 62 #define PRESENT_TEMPERATURE 0x2E //R - -
dev_num_ecam 0:6e72b34b96c5 63 #define REGISTERED_INSTRUCTION 0x2F //R 0 -
dev_num_ecam 0:6e72b34b96c5 64 #define MOVING 0x31 //R 0 -
dev_num_ecam 0:6e72b34b96c5 65 #define HARDWARE_ERROR_STATUS 0x32 //R 0 -
dev_num_ecam 0:6e72b34b96c5 66 #define PUNCH 0x33 //RW 32 0 ~ 1023
dev_num_ecam 0:6e72b34b96c5 67
dev_num_ecam 0:6e72b34b96c5 68 ////////// INSTRUCTION ///////////
dev_num_ecam 0:6e72b34b96c5 69 #define PING 0x01
dev_num_ecam 0:6e72b34b96c5 70 #define READ_DATA 0x02
dev_num_ecam 0:6e72b34b96c5 71 #define WRITE_DATA 0x03
dev_num_ecam 0:6e72b34b96c5 72 #define REG_WRITE 0x04
dev_num_ecam 0:6e72b34b96c5 73 #define ACTION 0x05
dev_num_ecam 0:6e72b34b96c5 74 #define FACTORY_RESET 0x06
dev_num_ecam 0:6e72b34b96c5 75 #define REBOOT 0x08
dev_num_ecam 0:6e72b34b96c5 76 #define STATUS 0x55
dev_num_ecam 0:6e72b34b96c5 77 #define SYNC_READ 0x82
dev_num_ecam 0:6e72b34b96c5 78 #define SYNC_WRITE 0x83
dev_num_ecam 0:6e72b34b96c5 79 #define BULK_READ 0x92
dev_num_ecam 0:6e72b34b96c5 80 #define BULK_WRITE 0x93
dev_num_ecam 0:6e72b34b96c5 81
dev_num_ecam 0:6e72b34b96c5 82 ////////// ERROR ///////////
dev_num_ecam 0:6e72b34b96c5 83 #define RESULT_FAIL 0x01
dev_num_ecam 0:6e72b34b96c5 84 #define INS_ERR 0x02
dev_num_ecam 0:6e72b34b96c5 85 #define CRC_ERR 0x03
dev_num_ecam 0:6e72b34b96c5 86 #define DATA_RANGE_ERR 0x04
dev_num_ecam 0:6e72b34b96c5 87 #define DATA_LENGTH_ERR 0x05
dev_num_ecam 0:6e72b34b96c5 88 #define DATA_LIMIT_ERR 0x06
dev_num_ecam 0:6e72b34b96c5 89 #define ACCESS_ERR 0x07
dev_num_ecam 0:6e72b34b96c5 90
dev_num_ecam 0:6e72b34b96c5 91 ////////// RESET ///////////
dev_num_ecam 0:6e72b34b96c5 92 #define RESET_ALL 0xFF
dev_num_ecam 0:6e72b34b96c5 93 #define RESET_EXP_ID 0x01
dev_num_ecam 0:6e72b34b96c5 94 #define RESET_EXP_ID_BR 0x02
dev_num_ecam 0:6e72b34b96c5 95
dev_num_ecam 0:6e72b34b96c5 96 class XL320
dev_num_ecam 0:6e72b34b96c5 97 {
dev_num_ecam 0:6e72b34b96c5 98 public:
dev_num_ecam 0:6e72b34b96c5 99 XL320(uint32_t baud, PinName tx, PinName rx);
dev_num_ecam 0:6e72b34b96c5 100 ~XL320();
dev_num_ecam 0:6e72b34b96c5 101
dev_num_ecam 0:6e72b34b96c5 102 ////// EEPROM ACCESS METHODS //////
dev_num_ecam 0:6e72b34b96c5 103
dev_num_ecam 0:6e72b34b96c5 104 /***** XL320 Network Parameter *****/
dev_num_ecam 0:6e72b34b96c5 105 void SetID(uint8_t id, uint8_t newId);
dev_num_ecam 0:6e72b34b96c5 106 uint8_t GetID(uint8_t id);
dev_num_ecam 0:6e72b34b96c5 107 void SetBaudRate(uint8_t id, uint8_t baudrate);
dev_num_ecam 0:6e72b34b96c5 108 uint8_t GetBaudRate(uint8_t id);
dev_num_ecam 0:6e72b34b96c5 109 void SetRetDelTime(uint8_t id, uint8_t time);
dev_num_ecam 0:6e72b34b96c5 110 uint8_t GetRetDelTime(uint8_t id);
dev_num_ecam 0:6e72b34b96c5 111
dev_num_ecam 0:6e72b34b96c5 112 /***** XL320 Motor Setting *****/
dev_num_ecam 0:6e72b34b96c5 113 void SetCWAngLim(uint8_t id, uint16_t angle);
dev_num_ecam 0:6e72b34b96c5 114 uint16_t GetCWAngLim(uint8_t id);
dev_num_ecam 0:6e72b34b96c5 115 void SetCCWAngLim(uint8_t id, uint16_t angle);
dev_num_ecam 0:6e72b34b96c5 116 uint16_t GetCCWAngLim(uint8_t id);
dev_num_ecam 0:6e72b34b96c5 117 void SetContMode(uint8_t id, uint8_t mode);
dev_num_ecam 0:6e72b34b96c5 118 uint8_t GetContMode(uint8_t id);
dev_num_ecam 0:6e72b34b96c5 119 void SetTempLim(uint8_t id, uint8_t temp);
dev_num_ecam 0:6e72b34b96c5 120 uint8_t GetTempLim(uint8_t id);
dev_num_ecam 0:6e72b34b96c5 121 void SetLowVoltLim(uint8_t id, uint8_t volt);
dev_num_ecam 0:6e72b34b96c5 122 uint8_t GetLowVoltLim(uint8_t id);
dev_num_ecam 0:6e72b34b96c5 123 void SetUpVoltLim(uint8_t id, uint8_t volt);
dev_num_ecam 0:6e72b34b96c5 124 uint8_t GetUpVoltLim(uint8_t id);
dev_num_ecam 0:6e72b34b96c5 125 void SetMaxTorq(uint8_t id, uint16_t torque);
dev_num_ecam 0:6e72b34b96c5 126 uint16_t GetMaxTorq(uint8_t id);
dev_num_ecam 0:6e72b34b96c5 127 void SetRetLev(uint8_t id, uint8_t level);
dev_num_ecam 0:6e72b34b96c5 128 uint8_t GetRetLev(uint8_t id);
dev_num_ecam 0:6e72b34b96c5 129 void SetAlarmShut(uint8_t id, uint8_t alarm);
dev_num_ecam 0:6e72b34b96c5 130 uint8_t GetAlarmShut(uint8_t id);
dev_num_ecam 0:6e72b34b96c5 131
dev_num_ecam 0:6e72b34b96c5 132 ////// RAM ACCESS METHODS //////
dev_num_ecam 0:6e72b34b96c5 133
dev_num_ecam 0:6e72b34b96c5 134 /***** XL320 On/Off *****/
dev_num_ecam 0:6e72b34b96c5 135 void SetTorqueEn(uint8_t id, uint8_t enable);
dev_num_ecam 0:6e72b34b96c5 136 uint8_t GetTorqueEn(uint8_t id);
dev_num_ecam 0:6e72b34b96c5 137 void TurnOnLED(uint8_t id, uint8_t led);
dev_num_ecam 0:6e72b34b96c5 138 uint8_t GetStatusLED(uint8_t id);
dev_num_ecam 0:6e72b34b96c5 139
dev_num_ecam 0:6e72b34b96c5 140 /***** XL320 Motor Control *****/
dev_num_ecam 0:6e72b34b96c5 141 void SetDGain(uint8_t id, uint8_t d_cons);
dev_num_ecam 0:6e72b34b96c5 142 uint8_t GetDGain(uint8_t id);
dev_num_ecam 0:6e72b34b96c5 143 void SetIGain(uint8_t id, uint8_t i_cons);
dev_num_ecam 0:6e72b34b96c5 144 uint8_t GetIGain(uint8_t id);
dev_num_ecam 0:6e72b34b96c5 145 void SetPGain(uint8_t id, uint8_t p_cons);
dev_num_ecam 0:6e72b34b96c5 146 uint8_t GetPGain(uint8_t id);
dev_num_ecam 0:6e72b34b96c5 147 void SetGoalPos(uint8_t id, uint16_t position);
dev_num_ecam 0:6e72b34b96c5 148 uint16_t GetGoalPos(uint8_t id);
dev_num_ecam 0:6e72b34b96c5 149 void SetGoalVel(uint8_t id, uint16_t velocity);
dev_num_ecam 0:6e72b34b96c5 150 uint16_t GetGoalVel(uint8_t id);
dev_num_ecam 0:6e72b34b96c5 151 void SetGoalTorq(uint8_t id, uint16_t torque);
dev_num_ecam 0:6e72b34b96c5 152 uint16_t GetGoalTorq(uint8_t id);
dev_num_ecam 0:6e72b34b96c5 153
dev_num_ecam 0:6e72b34b96c5 154 /***** XL320 Feedback *****/
dev_num_ecam 0:6e72b34b96c5 155 uint16_t GetPresentPos(uint8_t id);
dev_num_ecam 0:6e72b34b96c5 156 uint16_t GetPresentSpeed(uint8_t id);
dev_num_ecam 0:6e72b34b96c5 157 uint16_t GetPresentLoad(uint8_t id);
dev_num_ecam 0:6e72b34b96c5 158 uint8_t GetPresentVolt(uint8_t id);
dev_num_ecam 0:6e72b34b96c5 159 uint8_t GetPresentTemp(uint8_t id);
dev_num_ecam 0:6e72b34b96c5 160
dev_num_ecam 0:6e72b34b96c5 161 /***** XL320 Status *****/
dev_num_ecam 0:6e72b34b96c5 162 uint8_t GetRegInst(uint8_t id);
dev_num_ecam 0:6e72b34b96c5 163 uint8_t GetMoving(uint8_t id);
dev_num_ecam 0:6e72b34b96c5 164 uint8_t GetHWErr(uint8_t id);
dev_num_ecam 0:6e72b34b96c5 165 void SetPunch(uint8_t id, uint16_t punch);
dev_num_ecam 0:6e72b34b96c5 166 uint16_t GetPunch(uint8_t id);
dev_num_ecam 0:6e72b34b96c5 167
dev_num_ecam 0:6e72b34b96c5 168 /***** XL320 Instruction Method *****/
dev_num_ecam 0:6e72b34b96c5 169 void FactoryReset(uint8_t id, uint8_t option);
dev_num_ecam 0:6e72b34b96c5 170 void Ping(uint8_t id);
dev_num_ecam 0:6e72b34b96c5 171
dev_num_ecam 0:6e72b34b96c5 172 private:
dev_num_ecam 0:6e72b34b96c5 173 /***** Generic functions *****/
dev_num_ecam 0:6e72b34b96c5 174 void SetSomething(uint8_t id, uint16_t address, uint8_t param[], uint8_t paramLength);
dev_num_ecam 0:6e72b34b96c5 175 uint8_t GetSomething(uint8_t id, uint16_t address);
dev_num_ecam 0:6e72b34b96c5 176 uint16_t GetSomethingElse(uint8_t id, uint16_t address);
dev_num_ecam 0:6e72b34b96c5 177
dev_num_ecam 0:6e72b34b96c5 178 /***** XL320 Instruction Method *****/
dev_num_ecam 0:6e72b34b96c5 179 void iRead(uint8_t id, uint8_t length, uint16_t address);
dev_num_ecam 0:6e72b34b96c5 180 void iWrite(uint8_t id, uint16_t address, uint8_t param[], uint8_t paramLength);
dev_num_ecam 0:6e72b34b96c5 181
dev_num_ecam 0:6e72b34b96c5 182 /***** Dynamixel Protocol 2.0 Methods *****/
dev_num_ecam 0:6e72b34b96c5 183 void sendIPacket();
dev_num_ecam 0:6e72b34b96c5 184 void getRPacket();
dev_num_ecam 0:6e72b34b96c5 185
dev_num_ecam 0:6e72b34b96c5 186 /***** Calculating CRC Method *****/
dev_num_ecam 0:6e72b34b96c5 187 unsigned short update_crc(unsigned short crc_accum, unsigned char *data_blk_ptr, unsigned short data_blk_size);
dev_num_ecam 0:6e72b34b96c5 188
dev_num_ecam 0:6e72b34b96c5 189 /***** Variables *****/
dev_num_ecam 0:6e72b34b96c5 190 uint32_t baudrate; //Baudrate
dev_num_ecam 0:6e72b34b96c5 191 uint8_t iPacket[20]; //Instruction Packet
dev_num_ecam 0:6e72b34b96c5 192 uint8_t iPacketLength; //Instruction Packet Length
dev_num_ecam 0:6e72b34b96c5 193 uint8_t rPacket[20]; //Return Packet
dev_num_ecam 0:6e72b34b96c5 194 uint8_t rPacketLength; //Return Packet Length
dev_num_ecam 0:6e72b34b96c5 195
dev_num_ecam 0:6e72b34b96c5 196 Serial sXL320; //Serial Line for XL320
dev_num_ecam 0:6e72b34b96c5 197 };
dev_num_ecam 0:6e72b34b96c5 198
dev_num_ecam 0:6e72b34b96c5 199 #endif