Roger Weng / MX28

dyamixel motor library

Fork of MX28 by LDSC_Robotics_TAs

Mx28.h@3:e221394e585b, 2017-07-17 (annotated)

Committer:
roger5641
Date:
Mon Jul 17 02:19:12 2017 +0000
Revision:
3:e221394e585b
Parent:
2:2e32ee9f0e51
Child:
4:ffe33f18ab21
work

Who changed what in which revision?

UserRevisionLine numberNew contents of line
gert_lauritsen 0:45d73665e91f 1 /*
gert_lauritsen 0:45d73665e91f 2 How Dynamixel work can be found
gert_lauritsen 0:45d73665e91f 3 --------------------------------
gert_lauritsen 0:45d73665e91f 4 Robotis e-Manual
gert_lauritsen 0:45d73665e91f 5 http://support.robotis.com
gert_lauritsen 0:45d73665e91f 6
gert_lauritsen 0:45d73665e91f 7 Overview of Communication
gert_lauritsen 0:45d73665e91f 8 http://support.robotis.com/en/product/dynamixel/dxl_communication.htm
gert_lauritsen 0:45d73665e91f 9
gert_lauritsen 0:45d73665e91f 10 Kind of Instruction
gert_lauritsen 0:45d73665e91f 11 http://support.robotis.com/en/product/dynamixel/communication/dxl_instruction.htm
gert_lauritsen 0:45d73665e91f 12
gert_lauritsen 0:45d73665e91f 13 Instruction Packet & Status Packet (Return Packet)
gert_lauritsen 0:45d73665e91f 14 http://support.robotis.com/en/product/dynamixel/communication/dxl_packet.htm
gert_lauritsen 0:45d73665e91f 15
gert_lauritsen 0:45d73665e91f 16 Control Table
gert_lauritsen 0:45d73665e91f 17 http://support.robotis.com/en/product/dynamixel/mx_series/mx-28.htm
gert_lauritsen 0:45d73665e91f 18
gert_lauritsen 0:45d73665e91f 19 */
gert_lauritsen 0:45d73665e91f 20
gert_lauritsen 0:45d73665e91f 21 #ifndef Mx28_h
gert_lauritsen 0:45d73665e91f 22 #define Mx28_h
gert_lauritsen 0:45d73665e91f 23
gert_lauritsen 0:45d73665e91f 24 //-------------------------------------------------------------------------------------------------------------------------------
gert_lauritsen 0:45d73665e91f 25 // define - Dynamixel Hex code table
gert_lauritsen 0:45d73665e91f 26 //-------------------------------------------------------------------------------------------------------------------------------
gert_lauritsen 0:45d73665e91f 27 // EEPROM AREA
gert_lauritsen 0:45d73665e91f 28 #define EEPROM_MODEL_NUMBER_L 0x00
gert_lauritsen 0:45d73665e91f 29 #define EEPROM_MODEL_NUMBER_H 0x01
gert_lauritsen 0:45d73665e91f 30 #define EEPROM_VERSION 0x02
gert_lauritsen 0:45d73665e91f 31 #define EEPROM_ID 0x03
gert_lauritsen 0:45d73665e91f 32 #define EEPROM_BAUD_RATE 0x04
gert_lauritsen 0:45d73665e91f 33 #define EEPROM_RETURN_DELAY_TIME 0x05
gert_lauritsen 0:45d73665e91f 34 #define EEPROM_CW_ANGLE_LIMIT_L 0x06
gert_lauritsen 0:45d73665e91f 35 #define EEPROM_CW_ANGLE_LIMIT_H 0x07
gert_lauritsen 0:45d73665e91f 36 #define EEPROM_CCW_ANGLE_LIMIT_L 0x08
gert_lauritsen 0:45d73665e91f 37 #define EEPROM_CCW_ANGLE_LIMIT_H 0x09
gert_lauritsen 0:45d73665e91f 38 #define EEPROM_LIMIT_TEMPERATURE 0x0B
gert_lauritsen 0:45d73665e91f 39 #define EEPROM_LOW_LIMIT_VOLTAGE 0x0C
gert_lauritsen 0:45d73665e91f 40 #define EEPROM_HIGN_LIMIT_VOLTAGE 0x0D
gert_lauritsen 0:45d73665e91f 41 #define EEPROM_MAX_TORQUE_L 0x0E
gert_lauritsen 0:45d73665e91f 42 #define EEPROM_MAX_TORQUE_H 0x0F
gert_lauritsen 0:45d73665e91f 43 #define EEPROM_RETURN_LEVEL 0x10
gert_lauritsen 0:45d73665e91f 44 #define EEPROM_ALARM_LED 0x11
gert_lauritsen 0:45d73665e91f 45 #define EEPROM_ALARM_SHUTDOWN 0x12
gert_lauritsen 0:45d73665e91f 46 // RAM AREA
gert_lauritsen 0:45d73665e91f 47 #define RAM_TORQUE_ENABLE 0x18
gert_lauritsen 0:45d73665e91f 48 #define RAM_LED 0x19
gert_lauritsen 0:45d73665e91f 49 #define RAM_PROPORTIONAL_GAIN 0x1A
gert_lauritsen 0:45d73665e91f 50 #define RAM_INTERGRAL_GAIN 0x1B
gert_lauritsen 0:45d73665e91f 51 #define RAM_DERIVATIVE_GAIN 0x1C
gert_lauritsen 0:45d73665e91f 52 #define RAM_GOAL_POSITION_L 0x1E
gert_lauritsen 0:45d73665e91f 53 #define RAM_GOAL_POSITION_H 0x1F
gert_lauritsen 0:45d73665e91f 54 #define RAM_GOAL_SPEED_L 0x20
gert_lauritsen 0:45d73665e91f 55 #define RAM_GOAL_SPEED_H 0x21
gert_lauritsen 0:45d73665e91f 56 #define RAM_TORQUE_LIMIT_L 0x22
gert_lauritsen 0:45d73665e91f 57 #define RAM_TORQUE_LIMIT_H 0x23
gert_lauritsen 0:45d73665e91f 58 #define RAM_PRESENT_POSITION_L 0x24
gert_lauritsen 0:45d73665e91f 59 #define RAM_PRESENT_POSITION_H 0x25
gert_lauritsen 0:45d73665e91f 60 #define RAM_PRESENT_SPEED_L 0x26
gert_lauritsen 0:45d73665e91f 61 #define RAM_PRESENT_SPEED_H 0x27
gert_lauritsen 0:45d73665e91f 62 #define RAM_PRESENT_LOAD_L 0x28
gert_lauritsen 0:45d73665e91f 63 #define RAM_PRESENT_LOAD_H 0x29
gert_lauritsen 0:45d73665e91f 64 #define RAM_PRESENT_VOLTAGE 0x2A
gert_lauritsen 0:45d73665e91f 65 #define RAM_PRESENT_TEMPERATURE 0x2B
gert_lauritsen 0:45d73665e91f 66 #define RAM_REGISTER 0x2C
gert_lauritsen 0:45d73665e91f 67 #define RAM_MOVING 0x2E
gert_lauritsen 0:45d73665e91f 68 #define RAM_LOCK 0x2F
gert_lauritsen 0:45d73665e91f 69 #define RAM_PUNCH_L 0x30
gert_lauritsen 0:45d73665e91f 70 #define RAM_PUNCH_H 0x31
gert_lauritsen 0:45d73665e91f 71
weisnail 2:2e32ee9f0e51 72 #define RAM_TORQUE_CONTROL_MODE_ENABLE 0X46
weisnail 2:2e32ee9f0e51 73 #define RAM_GOAL_TORQUE_L 0X47
weisnail 2:2e32ee9f0e51 74 #define RAM_GOAL_TORQUE_H 0X48
gert_lauritsen 0:45d73665e91f 75
gert_lauritsen 0:45d73665e91f 76 //-------------------------------------------------------------------------------------------------------------------------------
gert_lauritsen 0:45d73665e91f 77 // Instruction commands Set
gert_lauritsen 0:45d73665e91f 78 //-------------------------------------------------------------------------------------------------------------------------------
gert_lauritsen 0:45d73665e91f 79 #define COMMAND_PING 0x01
gert_lauritsen 0:45d73665e91f 80 #define COMMAND_READ_DATA 0x02
gert_lauritsen 0:45d73665e91f 81 #define COMMAND_WRITE_DATA 0x03
gert_lauritsen 0:45d73665e91f 82 #define COMMAND_REG_WRITE_DATA 0x04
gert_lauritsen 0:45d73665e91f 83 #define COMMAND_ACTION 0x05
gert_lauritsen 0:45d73665e91f 84 #define COMMAND_RESET 0x06
gert_lauritsen 0:45d73665e91f 85 #define COMMAND_SYNC_WRITE 0x83
gert_lauritsen 0:45d73665e91f 86
gert_lauritsen 0:45d73665e91f 87
gert_lauritsen 0:45d73665e91f 88 //-------------------------------------------------------------------------------------------------------------------------------
gert_lauritsen 0:45d73665e91f 89 //Instruction packet lengths
gert_lauritsen 0:45d73665e91f 90 #define READ_ONE_BYTE_LENGTH 0x01
gert_lauritsen 0:45d73665e91f 91 #define READ_TWO_BYTE_LENGTH 0x02
gert_lauritsen 0:45d73665e91f 92 #define RESET_LENGTH 0x02
gert_lauritsen 0:45d73665e91f 93 #define PING_LENGTH 0x02
gert_lauritsen 0:45d73665e91f 94 #define ACTION_LENGTH 0x02
gert_lauritsen 0:45d73665e91f 95 #define SET_ID_LENGTH 0x04
gert_lauritsen 0:45d73665e91f 96 #define SET_BD_LENGTH 0x04
gert_lauritsen 0:45d73665e91f 97 #define SET_RETURN_LEVEL_LENGTH 0x04
gert_lauritsen 0:45d73665e91f 98 #define READ_TEMP_LENGTH 0x04
gert_lauritsen 0:45d73665e91f 99 #define READ_POS_LENGTH 0x04
gert_lauritsen 0:45d73665e91f 100 #define READ_LOAD_LENGTH 0x04
gert_lauritsen 0:45d73665e91f 101 #define READ_SPEED_LENGTH 0x04
gert_lauritsen 0:45d73665e91f 102 #define READ_VOLT_LENGTH 0x04
gert_lauritsen 0:45d73665e91f 103 #define READ_REGISTER_LENGTH 0x04
gert_lauritsen 0:45d73665e91f 104 #define READ_MOVING_LENGTH 0x04
gert_lauritsen 0:45d73665e91f 105 #define READ_LOCK_LENGTH 0x04
gert_lauritsen 0:45d73665e91f 106 #define LED_LENGTH 0x04
gert_lauritsen 0:45d73665e91f 107 #define SET_HOLDING_TORQUE_LENGTH 0x04
gert_lauritsen 0:45d73665e91f 108 #define SET_MAX_TORQUE_LENGTH 0x05
gert_lauritsen 0:45d73665e91f 109 #define SET_ALARM_LENGTH 0x04
gert_lauritsen 0:45d73665e91f 110 #define READ_LOAD_LENGTH 0x04
gert_lauritsen 0:45d73665e91f 111 #define SET_RETURN_LENGTH 0x04
gert_lauritsen 0:45d73665e91f 112 #define WHEEL_LENGTH 0x05
gert_lauritsen 0:45d73665e91f 113 #define SERVO_GOAL_LENGTH 0x07
gert_lauritsen 0:45d73665e91f 114 #define SET_MODE_LENGTH 0x07
gert_lauritsen 0:45d73665e91f 115 #define SET_PUNCH_LENGTH 0x04
gert_lauritsen 0:45d73665e91f 116 #define SET_PID_LENGTH 0x06
gert_lauritsen 0:45d73665e91f 117 #define SET_TEMP_LENGTH 0x04
gert_lauritsen 0:45d73665e91f 118 #define SET_VOLT_LENGTH 0x05
gert_lauritsen 0:45d73665e91f 119 #define SYNC_LOAD_LENGTH 0x0D
gert_lauritsen 0:45d73665e91f 120 #define SYNC_DATA_LENGTH 0x02
gert_lauritsen 0:45d73665e91f 121
gert_lauritsen 0:45d73665e91f 122
gert_lauritsen 0:45d73665e91f 123 //-------------------------------------------------------------------------------------------------------------------------------
gert_lauritsen 0:45d73665e91f 124 // Specials
gert_lauritsen 0:45d73665e91f 125 //-------------------------------------------------------------------------------------------------------------------------------
gert_lauritsen 0:45d73665e91f 126
gert_lauritsen 0:45d73665e91f 127 #define OFF 0x00
gert_lauritsen 0:45d73665e91f 128 #define ON 0x01
gert_lauritsen 0:45d73665e91f 129
gert_lauritsen 0:45d73665e91f 130 #define SERVO 0x01
gert_lauritsen 0:45d73665e91f 131 #define WHEEL 0x00
gert_lauritsen 0:45d73665e91f 132
gert_lauritsen 0:45d73665e91f 133 #define LEFT 0x00
gert_lauritsen 0:45d73665e91f 134 #define RIGHT 0x01
gert_lauritsen 0:45d73665e91f 135
gert_lauritsen 0:45d73665e91f 136 #define NONE 0x00
gert_lauritsen 0:45d73665e91f 137 #define READ 0x01
gert_lauritsen 0:45d73665e91f 138 #define ALL 0x02
gert_lauritsen 0:45d73665e91f 139
gert_lauritsen 0:45d73665e91f 140 #define BROADCAST_ID 0xFE
gert_lauritsen 0:45d73665e91f 141
gert_lauritsen 0:45d73665e91f 142 #define HEADER 0xFF
gert_lauritsen 0:45d73665e91f 143
roger5641 3:e221394e585b 144 #define STATUS_PACKET_TIMEOUT 5 // in millis()
gert_lauritsen 0:45d73665e91f 145 #define STATUS_FRAME_BUFFER 5
gert_lauritsen 0:45d73665e91f 146
gert_lauritsen 0:45d73665e91f 147
gert_lauritsen 0:45d73665e91f 148
gert_lauritsen 0:45d73665e91f 149 class DynamixelClass {
gert_lauritsen 0:45d73665e91f 150 private:
gert_lauritsen 0:45d73665e91f 151 DigitalOut *servoSerialDir;
gert_lauritsen 0:45d73665e91f 152 Serial *servoSerial;
gert_lauritsen 0:45d73665e91f 153 void transmitInstructionPacket(void);
gert_lauritsen 0:45d73665e91f 154 unsigned int readStatusPacket(void);
gert_lauritsen 0:45d73665e91f 155 bool overflow;
gert_lauritsen 1:c866ce96ceb3 156 void debugframe(void);
gert_lauritsen 1:c866ce96ceb3 157 void debugStatusframe(void);
gert_lauritsen 1:c866ce96ceb3 158 DigitalOut *led2;
gert_lauritsen 0:45d73665e91f 159 public:
gert_lauritsen 0:45d73665e91f 160
gert_lauritsen 0:45d73665e91f 161 DynamixelClass(int baud,PinName D_Pin,PinName tx, PinName rx); //Constructor
gert_lauritsen 0:45d73665e91f 162 ~DynamixelClass(void); //destruktor
gert_lauritsen 0:45d73665e91f 163
gert_lauritsen 0:45d73665e91f 164 void NewBaudRate(int baud);
gert_lauritsen 0:45d73665e91f 165 unsigned int reset(unsigned char);
gert_lauritsen 0:45d73665e91f 166 unsigned int ping(unsigned char);
gert_lauritsen 0:45d73665e91f 167
gert_lauritsen 0:45d73665e91f 168 unsigned int setStatusPaketReturnDelay(unsigned char,unsigned char);
gert_lauritsen 0:45d73665e91f 169 unsigned int setID(unsigned char, unsigned char);
gert_lauritsen 0:45d73665e91f 170 unsigned int setBaudRate(unsigned char, long);
gert_lauritsen 0:45d73665e91f 171 unsigned int setMaxTorque(unsigned char, int);
gert_lauritsen 0:45d73665e91f 172 unsigned int setHoldingTorque(unsigned char, bool);
gert_lauritsen 0:45d73665e91f 173 unsigned int setAlarmShutdown(unsigned char,unsigned char);
gert_lauritsen 0:45d73665e91f 174 unsigned int setStatusPaket(unsigned char,unsigned char);
gert_lauritsen 0:45d73665e91f 175 unsigned int setMode(unsigned char, bool, unsigned int, unsigned int);
gert_lauritsen 0:45d73665e91f 176 unsigned int setPunch(unsigned char,unsigned int);
gert_lauritsen 0:45d73665e91f 177 unsigned int setPID(unsigned char,unsigned char,unsigned char,unsigned char);
gert_lauritsen 0:45d73665e91f 178 unsigned int setTemp(unsigned char,unsigned char);
gert_lauritsen 0:45d73665e91f 179 unsigned int setVoltage(unsigned char,unsigned char,unsigned char);
gert_lauritsen 0:45d73665e91f 180
gert_lauritsen 0:45d73665e91f 181 unsigned int servo(unsigned char, unsigned int, unsigned int);
gert_lauritsen 0:45d73665e91f 182 unsigned int servoPreload(unsigned char, unsigned int, unsigned int);
gert_lauritsen 0:45d73665e91f 183 unsigned int wheel(unsigned char, bool, unsigned int);
gert_lauritsen 0:45d73665e91f 184 void wheelSync(unsigned char,bool,unsigned int,unsigned char, bool,unsigned int,unsigned char, bool,unsigned int);
gert_lauritsen 0:45d73665e91f 185 unsigned int wheelPreload(unsigned char, bool, unsigned int);
gert_lauritsen 0:45d73665e91f 186
gert_lauritsen 0:45d73665e91f 187 unsigned int action(unsigned char);
gert_lauritsen 0:45d73665e91f 188
gert_lauritsen 0:45d73665e91f 189 unsigned int readTemperature(unsigned char);
gert_lauritsen 0:45d73665e91f 190 unsigned int readVoltage(unsigned char);
gert_lauritsen 0:45d73665e91f 191 unsigned int readPosition(unsigned char);
gert_lauritsen 0:45d73665e91f 192 unsigned int readLoad(unsigned char);
gert_lauritsen 0:45d73665e91f 193 unsigned int readSpeed(unsigned char);
gert_lauritsen 0:45d73665e91f 194
gert_lauritsen 0:45d73665e91f 195 unsigned int checkRegister(unsigned char);
gert_lauritsen 0:45d73665e91f 196 unsigned int checkMovement(unsigned char);
gert_lauritsen 0:45d73665e91f 197 unsigned int checkLock(unsigned char);
gert_lauritsen 0:45d73665e91f 198
gert_lauritsen 0:45d73665e91f 199 unsigned int ledState(unsigned char, bool);
weisnail 2:2e32ee9f0e51 200
weisnail 2:2e32ee9f0e51 201 unsigned int torqueMode(unsigned char ID, bool Status);
weisnail 2:2e32ee9f0e51 202 unsigned int torque(unsigned char ID,unsigned int Torque);
weisnail 2:2e32ee9f0e51 203 unsigned int readRegister(unsigned char ID,unsigned char Register);
gert_lauritsen 0:45d73665e91f 204 };
gert_lauritsen 0:45d73665e91f 205
gert_lauritsen 0:45d73665e91f 206 #endif