Rhino Reimburse / Mbed 2 deprecated Dynamixel_XL320

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

Fork of XL320_Test by Rhino Reimburse

XL320_MFA/XL320_MFA.h@4:2df002cf5f6c, 2016-02-28 (annotated)

Committer:
mfajaradityo
Date:
Sun Feb 28 14:48:03 2016 +0000
Revision:
4:2df002cf5f6c
Parent:
3:89bed7a6d852 
Benerin deskripsi repository yang sebelumnya

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mfajaradityo 0:2071f8e463dd 1 //Author : Muhammad Fajar Adityo
mfajaradityo 0:2071f8e463dd 2 //This file is based on Arduino Library for Dynamixel XL-320 Servo found somewhere in the Internet
mfajaradityo 0:2071f8e463dd 3
mfajaradityo 0:2071f8e463dd 4 #ifndef XL320_H
mfajaradityo 0:2071f8e463dd 5 #define XL320_H
mfajaradityo 0:2071f8e463dd 6
mfajaradityo 0:2071f8e463dd 7 #include "mbed.h"
mfajaradityo 0:2071f8e463dd 8
mfajaradityo 0:2071f8e463dd 9 static Serial pc(USBTX,USBRX);
mfajaradityo 2:0ec2dce7c5f8 10 ////////// DEBUG ///////////
mfajaradityo 4:2df002cf5f6c 11 #define DATA_DEBUG //Command this line to disable some of pc.printf in XL320_MFA.cpp
mfajaradityo 0:2071f8e463dd 12
mfajaradityo 0:2071f8e463dd 13 ////////// SHIFT BYTES ///////////
mfajaradityo 0:2071f8e463dd 14 #define SHIFT_TO_LSB(w) (uint8_t((uint16_t(w)) & 0x00ff))
mfajaradityo 0:2071f8e463dd 15 #define SHIFT_TO_MSB(w) (uint8_t((uint16_t(w) >> 8) & 0x00ff))
mfajaradityo 0:2071f8e463dd 16
mfajaradityo 0:2071f8e463dd 17 ////////// EEPROM ///////////
mfajaradityo 0:2071f8e463dd 18 #define MODEL_NUMBER 0
mfajaradityo 0:2071f8e463dd 19 #define VERSION 2
mfajaradityo 0:2071f8e463dd 20 #define XL_ID 3
mfajaradityo 0:2071f8e463dd 21 #define BAUD_RATE 4 //0: 9600, 1:57600, 2:115200, 3:1Mbps
mfajaradityo 0:2071f8e463dd 22 #define RETURN_DELAY_TIME 5
mfajaradityo 0:2071f8e463dd 23 #define CW_ANGLE_LIMIT 6
mfajaradityo 0:2071f8e463dd 24 #define CCW_ANGLE_LIMIT 8
mfajaradityo 0:2071f8e463dd 25 #define SYSTEM_DATA2 10
mfajaradityo 0:2071f8e463dd 26 #define CONTROL_MODE 11
mfajaradityo 0:2071f8e463dd 27 #define LIMIT_TEMPERATURE 12
mfajaradityo 0:2071f8e463dd 28 #define LOWER_LIMIT_VOLTAGE 13
mfajaradityo 0:2071f8e463dd 29 #define UPPER_LIMIT_VOLTAGE 14
mfajaradityo 0:2071f8e463dd 30 #define MAX_TORQUE 15
mfajaradityo 0:2071f8e463dd 31 #define RETURN_LEVEL 17
mfajaradityo 0:2071f8e463dd 32 #define ALARM_SHUTDOWN 18
mfajaradityo 0:2071f8e463dd 33
mfajaradityo 0:2071f8e463dd 34 ////////// RAM ///////////
mfajaradityo 0:2071f8e463dd 35 #define TORQUE_ENABLE 24
mfajaradityo 0:2071f8e463dd 36 #define LED 25
mfajaradityo 0:2071f8e463dd 37 #define D_GAIN 27
mfajaradityo 0:2071f8e463dd 38 #define I_GAIN 28
mfajaradityo 0:2071f8e463dd 39 #define P_GAIN 29
mfajaradityo 0:2071f8e463dd 40 #define GOAL_POSITION 30
mfajaradityo 0:2071f8e463dd 41 #define GOAL_SPEED 32
mfajaradityo 0:2071f8e463dd 42 #define GOAL_TORQUE 35
mfajaradityo 0:2071f8e463dd 43 #define PRESENT_POSITION 37
mfajaradityo 0:2071f8e463dd 44 #define PRESENT_SPEED 39
mfajaradityo 0:2071f8e463dd 45 #define PRESENT_LOAD 41
mfajaradityo 0:2071f8e463dd 46 #define PRESENT_VOLTAGE 45
mfajaradityo 0:2071f8e463dd 47 #define PRESENT_TEMPERATURE 46
mfajaradityo 0:2071f8e463dd 48 #define REGISTERED_INSTRUCTION 47
mfajaradityo 0:2071f8e463dd 49 #define MOVING 49
mfajaradityo 0:2071f8e463dd 50 #define HARDWARE_ERROR_STATUS 50
mfajaradityo 0:2071f8e463dd 51 #define PUNCH 51
mfajaradityo 0:2071f8e463dd 52
mfajaradityo 0:2071f8e463dd 53 ////////// INSTRUCTION ///////////
mfajaradityo 0:2071f8e463dd 54 #define PING 1
mfajaradityo 0:2071f8e463dd 55 #define READ_DATA 2
mfajaradityo 0:2071f8e463dd 56 #define WRITE_DATA 3
mfajaradityo 0:2071f8e463dd 57 #define REG_WRITE 4
mfajaradityo 0:2071f8e463dd 58 #define ACTION 5
mfajaradityo 0:2071f8e463dd 59 #define FACTORY_RESET 6
mfajaradityo 0:2071f8e463dd 60 #define REBOOT 8
mfajaradityo 0:2071f8e463dd 61 #define STATUS 85
mfajaradityo 0:2071f8e463dd 62 #define SYNC_READ 130
mfajaradityo 0:2071f8e463dd 63 #define SYNC_WRITE 131
mfajaradityo 0:2071f8e463dd 64 #define BULK_READ 146
mfajaradityo 0:2071f8e463dd 65 #define BULK_WRITE 147
mfajaradityo 0:2071f8e463dd 66
mfajaradityo 0:2071f8e463dd 67 ////////// ERROR ///////////
mfajaradityo 0:2071f8e463dd 68 #define RESULT_FAIL 0x01
mfajaradityo 0:2071f8e463dd 69 #define INS_ERR 0x02
mfajaradityo 0:2071f8e463dd 70 #define CRC_ERR 0x03
mfajaradityo 0:2071f8e463dd 71 #define DATA_RANGE_ERR 0x04
mfajaradityo 0:2071f8e463dd 72 #define DATA_LENGTH_ERR 0x05
mfajaradityo 0:2071f8e463dd 73 #define DATA_LIMIT_ERR 0x06
mfajaradityo 0:2071f8e463dd 74 #define ACCESS_ERR 0x07
mfajaradityo 0:2071f8e463dd 75
mfajaradityo 0:2071f8e463dd 76 ////////// RESET ///////////
mfajaradityo 0:2071f8e463dd 77 #define RESET_ALL 0xFF
mfajaradityo 0:2071f8e463dd 78 #define RESET_EXP_ID 0x01
mfajaradityo 0:2071f8e463dd 79 #define RESET_EXP_ID_BR 0x02
mfajaradityo 0:2071f8e463dd 80
mfajaradityo 0:2071f8e463dd 81 class XL320{
mfajaradityo 0:2071f8e463dd 82
mfajaradityo 0:2071f8e463dd 83 public:
mfajaradityo 0:2071f8e463dd 84 XL320(uint32_t baudrate, PinName tx, PinName rx, PinName dirpin);
mfajaradityo 0:2071f8e463dd 85 ~XL320();
mfajaradityo 0:2071f8e463dd 86
mfajaradityo 0:2071f8e463dd 87 ////// EEPROM ACCESS METHODS //////
mfajaradityo 0:2071f8e463dd 88
mfajaradityo 0:2071f8e463dd 89 /***** XL320 Network Parameter *****/
mfajaradityo 0:2071f8e463dd 90 void SetID(uint8_t id,uint8_t idnew);
mfajaradityo 0:2071f8e463dd 91 uint8_t GetID(uint8_t id);
mfajaradityo 0:2071f8e463dd 92 void SetBaudRate(uint8_t id,uint8_t baudrate);
mfajaradityo 0:2071f8e463dd 93 uint8_t GetBaudRate(uint8_t id);
mfajaradityo 0:2071f8e463dd 94 void SetRetDelTime(uint8_t id,uint8_t time);
mfajaradityo 0:2071f8e463dd 95 uint8_t GetRetDelTime(uint8_t id);
mfajaradityo 0:2071f8e463dd 96
mfajaradityo 0:2071f8e463dd 97 /***** XL320 Motor Setting *****/
mfajaradityo 0:2071f8e463dd 98 void SetCWAngLim(uint8_t id,uint16_t angle);
mfajaradityo 0:2071f8e463dd 99 uint16_t GetCWAngLim(uint8_t id);
mfajaradityo 0:2071f8e463dd 100 void SetCCWAngLim(uint8_t id,uint16_t angle);
mfajaradityo 0:2071f8e463dd 101 uint16_t GetCCWAngLim(uint8_t id);
mfajaradityo 0:2071f8e463dd 102 void SetContMode(uint8_t id,uint8_t mode);
mfajaradityo 0:2071f8e463dd 103 uint8_t GetContMode(uint8_t id);
mfajaradityo 0:2071f8e463dd 104 void SetTempLim(uint8_t id,uint8_t temp);
mfajaradityo 0:2071f8e463dd 105 uint8_t GetTempLim(uint8_t id);
mfajaradityo 0:2071f8e463dd 106 void SetLowVoltLim(uint8_t id,uint8_t volt);
mfajaradityo 0:2071f8e463dd 107 uint8_t GetLowVoltLim(uint8_t id);
mfajaradityo 0:2071f8e463dd 108 void SetUpVoltLim(uint8_t id,uint8_t volt);
mfajaradityo 0:2071f8e463dd 109 uint8_t GetUpVoltLim(uint8_t id);
mfajaradityo 0:2071f8e463dd 110 void SetMaxTorq(uint8_t id,uint16_t torque);
mfajaradityo 0:2071f8e463dd 111 uint16_t GetMaxTorq(uint8_t id);
mfajaradityo 0:2071f8e463dd 112 void SetRetLev(uint8_t id,uint8_t level);
mfajaradityo 0:2071f8e463dd 113 uint8_t GetRetLev(uint8_t id);
mfajaradityo 0:2071f8e463dd 114 void SetAlarmShut(uint8_t id,uint8_t alarm);
mfajaradityo 0:2071f8e463dd 115 uint8_t GetAlarmShut(uint8_t id);
mfajaradityo 0:2071f8e463dd 116
mfajaradityo 0:2071f8e463dd 117 ////// RAM ACCESS METHODS //////
mfajaradityo 0:2071f8e463dd 118 /***** XL320 On/Off *****/
mfajaradityo 0:2071f8e463dd 119 void SetTorqueEn(uint8_t id,uint8_t enable);
mfajaradityo 0:2071f8e463dd 120 uint8_t GetTorqueEn(uint8_t id);
mfajaradityo 0:2071f8e463dd 121 void TurnOnLED(uint8_t id,uint8_t led);
mfajaradityo 0:2071f8e463dd 122 uint8_t GetStatusLED(uint8_t id);
mfajaradityo 0:2071f8e463dd 123
mfajaradityo 0:2071f8e463dd 124 /***** XL320 Motor Control *****/
mfajaradityo 0:2071f8e463dd 125 void SetDGain(uint8_t id,uint8_t d_cons);
mfajaradityo 0:2071f8e463dd 126 uint8_t GetDGain(uint8_t id);
mfajaradityo 0:2071f8e463dd 127 void SetIGain(uint8_t id,uint8_t i_cons);
mfajaradityo 0:2071f8e463dd 128 uint8_t GetIGain(uint8_t id);
mfajaradityo 0:2071f8e463dd 129 void SetPGain(uint8_t id,uint8_t p_cons);
mfajaradityo 0:2071f8e463dd 130 uint8_t GetPGain(uint8_t id);
mfajaradityo 0:2071f8e463dd 131 void SetGoalPos(uint8_t id,uint16_t position);
mfajaradityo 0:2071f8e463dd 132 uint16_t GetGoalPos(uint8_t id);
mfajaradityo 0:2071f8e463dd 133 void SetGoalVel(uint8_t id,uint16_t velocity);
mfajaradityo 0:2071f8e463dd 134 uint16_t GetGoalVel(uint8_t id);
mfajaradityo 0:2071f8e463dd 135 void SetGoalTorq(uint8_t id,uint16_t torque);
mfajaradityo 0:2071f8e463dd 136 uint16_t GetGoalTorq(uint8_t id);
mfajaradityo 0:2071f8e463dd 137
mfajaradityo 0:2071f8e463dd 138 /***** XL320 Feedback *****/
mfajaradityo 0:2071f8e463dd 139 uint16_t GetPresentPos(uint8_t id);
mfajaradityo 0:2071f8e463dd 140 uint16_t GetPresentSpeed(uint8_t id);
mfajaradityo 0:2071f8e463dd 141 uint16_t GetPresentLoad(uint8_t id);
mfajaradityo 0:2071f8e463dd 142 uint8_t GetPresentVolt(uint8_t id);
mfajaradityo 0:2071f8e463dd 143 uint8_t GetPresentTemp(uint8_t id);
mfajaradityo 0:2071f8e463dd 144
mfajaradityo 0:2071f8e463dd 145 /***** XL320 Status *****/
mfajaradityo 0:2071f8e463dd 146 uint8_t GetRegInst(uint8_t id);
mfajaradityo 0:2071f8e463dd 147 uint8_t GetMoving(uint8_t id);
mfajaradityo 0:2071f8e463dd 148 uint8_t GetHWErr(uint8_t id);
mfajaradityo 0:2071f8e463dd 149 void SetPunch(uint8_t id,uint16_t punch);
mfajaradityo 0:2071f8e463dd 150 uint16_t GetPunch(uint8_t id);
mfajaradityo 0:2071f8e463dd 151
mfajaradityo 0:2071f8e463dd 152 /***** XL320 Instruction Method *****/
mfajaradityo 0:2071f8e463dd 153 void iFactoryReset(uint8_t id,uint8_t option);
mfajaradityo 0:2071f8e463dd 154 void iPing(uint8_t id);
mfajaradityo 0:2071f8e463dd 155
mfajaradityo 0:2071f8e463dd 156 /***** Debugging *****/
mfajaradityo 0:2071f8e463dd 157 void printLastPackets();
mfajaradityo 0:2071f8e463dd 158 void translateLastRPacket();
mfajaradityo 0:2071f8e463dd 159
mfajaradityo 0:2071f8e463dd 160 private:
mfajaradityo 0:2071f8e463dd 161 /***** XL320 Instruction Method *****/
mfajaradityo 0:2071f8e463dd 162 void iRead(uint16_t length, uint16_t address);
mfajaradityo 0:2071f8e463dd 163 void iWrite(uint8_t length, uint16_t address, uint8_t param[]);
mfajaradityo 0:2071f8e463dd 164
mfajaradityo 0:2071f8e463dd 165 /***** Dynamixel Protocol 2.0 Methods *****/
mfajaradityo 0:2071f8e463dd 166 void sendIPacket();
mfajaradityo 0:2071f8e463dd 167 void getRPacket();
mfajaradityo 0:2071f8e463dd 168
mfajaradityo 0:2071f8e463dd 169 /***** Calculating CRC Method *****/
mfajaradityo 0:2071f8e463dd 170 unsigned short update_crc(unsigned short crc_accum, unsigned char *data_blk_ptr, unsigned short data_blk_size);
mfajaradityo 0:2071f8e463dd 171
mfajaradityo 0:2071f8e463dd 172 Serial sXL320; //Serial Line for XL320
mfajaradityo 0:2071f8e463dd 173
mfajaradityo 0:2071f8e463dd 174 const int nBytes = 20;
mfajaradityo 0:2071f8e463dd 175 uint8_t iPacket[nBytes]; //Instruction Packet //not optimal size?
mfajaradityo 0:2071f8e463dd 176 uint8_t rPacket[nBytes]; //Return Packet //not optimal size?
mfajaradityo 0:2071f8e463dd 177 uint8_t parameter[nBytes/2]; //Parameter for Instruction //not optimal size?
mfajaradityo 0:2071f8e463dd 178
mfajaradityo 0:2071f8e463dd 179 const int restTime = 500; //delay time before sending another Instruction Packet
mfajaradityo 0:2071f8e463dd 180 uint8_t targetID; //id of targeted XL320
mfajaradityo 0:2071f8e463dd 181 uint8_t targetInst; //instruction for targeted XL320
mfajaradityo 0:2071f8e463dd 182 uint16_t targetAddr; //address for instruction
mfajaradityo 0:2071f8e463dd 183 uint16_t CRC;
mfajaradityo 0:2071f8e463dd 184 uint16_t iPacketLength; //Instruction Packet Length
mfajaradityo 0:2071f8e463dd 185 uint16_t rPacketLength;
mfajaradityo 0:2071f8e463dd 186 uint16_t returnCRC;
mfajaradityo 0:2071f8e463dd 187 DigitalOut dPin; //Direction Pin = Enable Tx/RX Pin
mfajaradityo 0:2071f8e463dd 188
mfajaradityo 0:2071f8e463dd 189 };
mfajaradityo 0:2071f8e463dd 190
mfajaradityo 0:2071f8e463dd 191 #endif