dynamixel protocal by Liews Wuttipat.
Dynamixel.h@0:e4ba03a21caa, 2017-12-08 (annotated)
- Committer:
- sweilz
- Date:
- Fri Dec 08 23:32:57 2017 +0000
- Revision:
- 0:e4ba03a21caa
first commit.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
sweilz | 0:e4ba03a21caa | 1 | #include <mbed.h> |
sweilz | 0:e4ba03a21caa | 2 | |
sweilz | 0:e4ba03a21caa | 3 | //------------------------------------------------------------------------------------------------------------------------------- |
sweilz | 0:e4ba03a21caa | 4 | // define - Dynamixel Hex code table |
sweilz | 0:e4ba03a21caa | 5 | //------------------------------------------------------------------------------------------------------------------------------- |
sweilz | 0:e4ba03a21caa | 6 | // EEPROM AREA |
sweilz | 0:e4ba03a21caa | 7 | #define EEPROM_MODEL_NUMBER_L 0x00 |
sweilz | 0:e4ba03a21caa | 8 | #define EEPROM_MODEL_NUMBER_H 0x01 |
sweilz | 0:e4ba03a21caa | 9 | #define EEPROM_VERSION 0x02 |
sweilz | 0:e4ba03a21caa | 10 | #define EEPROM_ID 0x03 |
sweilz | 0:e4ba03a21caa | 11 | #define EEPROM_BAUD_RATE 0x04 |
sweilz | 0:e4ba03a21caa | 12 | #define EEPROM_RETURN_DELAY_TIME 0x05 |
sweilz | 0:e4ba03a21caa | 13 | #define EEPROM_CW_ANGLE_LIMIT_L 0x06 |
sweilz | 0:e4ba03a21caa | 14 | #define EEPROM_CW_ANGLE_LIMIT_H 0x07 |
sweilz | 0:e4ba03a21caa | 15 | #define EEPROM_CCW_ANGLE_LIMIT_L 0x08 |
sweilz | 0:e4ba03a21caa | 16 | #define EEPROM_CCW_ANGLE_LIMIT_H 0x09 |
sweilz | 0:e4ba03a21caa | 17 | #define EEPROM_DRIVE_MODE 0x0A |
sweilz | 0:e4ba03a21caa | 18 | #define EEPROM_LIMIT_TEMPERATURE 0x0B |
sweilz | 0:e4ba03a21caa | 19 | #define EEPROM_LOW_LIMIT_VOLTAGE 0x0C |
sweilz | 0:e4ba03a21caa | 20 | #define EEPROM_HIGN_LIMIT_VOLTAGE 0x0D |
sweilz | 0:e4ba03a21caa | 21 | #define EEPROM_MAX_TORQUE_L 0x0E |
sweilz | 0:e4ba03a21caa | 22 | #define EEPROM_MAX_TORQUE_H 0x0F |
sweilz | 0:e4ba03a21caa | 23 | #define EEPROM_RETURN_LEVEL 0x10 |
sweilz | 0:e4ba03a21caa | 24 | #define EEPROM_ALARM_LED 0x11 |
sweilz | 0:e4ba03a21caa | 25 | #define EEPROM_ALARM_SHUTDOWN 0x12 |
sweilz | 0:e4ba03a21caa | 26 | #define EEPROM_MULTITURN_L 0x14 |
sweilz | 0:e4ba03a21caa | 27 | #define EEPROM_MULTITURN_H 0x15 |
sweilz | 0:e4ba03a21caa | 28 | #define EEPROM_RESOLUTION_DIV 0x16 |
sweilz | 0:e4ba03a21caa | 29 | // RAM AREA |
sweilz | 0:e4ba03a21caa | 30 | #define RAM_TORQUE_ENABLE 0x18 |
sweilz | 0:e4ba03a21caa | 31 | #define RAM_LED 0x19 |
sweilz | 0:e4ba03a21caa | 32 | #define RAM_DERIVATIVE_GAIN 0x1A |
sweilz | 0:e4ba03a21caa | 33 | #define RAM_INTERGRAL_GAIN 0x1B |
sweilz | 0:e4ba03a21caa | 34 | #define RAM_PROPORTIONAL_GAIN 0x1C |
sweilz | 0:e4ba03a21caa | 35 | #define RAM_GOAL_POSITION_L 0x1E |
sweilz | 0:e4ba03a21caa | 36 | #define RAM_GOAL_POSITION_H 0x1F |
sweilz | 0:e4ba03a21caa | 37 | #define RAM_GOAL_SPEED_L 0x20 |
sweilz | 0:e4ba03a21caa | 38 | #define RAM_GOAL_SPEED_H 0x21 |
sweilz | 0:e4ba03a21caa | 39 | #define RAM_TORQUE_LIMIT_L 0x22 |
sweilz | 0:e4ba03a21caa | 40 | #define RAM_TORQUE_LIMIT_H 0x23 |
sweilz | 0:e4ba03a21caa | 41 | #define RAM_PRESENT_POSITION_L 0x24 |
sweilz | 0:e4ba03a21caa | 42 | #define RAM_PRESENT_POSITION_H 0x25 |
sweilz | 0:e4ba03a21caa | 43 | #define RAM_PRESENT_SPEED_L 0x26 |
sweilz | 0:e4ba03a21caa | 44 | #define RAM_PRESENT_SPEED_H 0x27 |
sweilz | 0:e4ba03a21caa | 45 | #define RAM_PRESENT_LOAD_L 0x28 |
sweilz | 0:e4ba03a21caa | 46 | #define RAM_PRESENT_LOAD_H 0x29 |
sweilz | 0:e4ba03a21caa | 47 | #define RAM_PRESENT_VOLTAGE 0x2A |
sweilz | 0:e4ba03a21caa | 48 | #define RAM_PRESENT_TEMPERATURE 0x2B |
sweilz | 0:e4ba03a21caa | 49 | #define RAM_REGISTER 0x2C |
sweilz | 0:e4ba03a21caa | 50 | #define RAM_MOVING 0x2E |
sweilz | 0:e4ba03a21caa | 51 | #define RAM_LOCK 0x2F |
sweilz | 0:e4ba03a21caa | 52 | #define RAM_PUNCH_L 0x30 |
sweilz | 0:e4ba03a21caa | 53 | #define RAM_PUNCH_H 0x31 |
sweilz | 0:e4ba03a21caa | 54 | #define RAM_SENSED_CURRENT_L 0x38 |
sweilz | 0:e4ba03a21caa | 55 | #define RAM_SENSED_CURRENT_H 0x39 |
sweilz | 0:e4ba03a21caa | 56 | #define RAM_CURRENT_L 0x44 |
sweilz | 0:e4ba03a21caa | 57 | #define RAM_CURRENT_H 0x45 |
sweilz | 0:e4ba03a21caa | 58 | #define RAM_TORQUE_CONTROL_MODE_ENABLE 0x46 |
sweilz | 0:e4ba03a21caa | 59 | #define RAM_GOAL_TORQUE_L 0x47 |
sweilz | 0:e4ba03a21caa | 60 | #define RAM_GOAL_TORQUE_H 0x48 |
sweilz | 0:e4ba03a21caa | 61 | #define RAM_GOAL_ACCELERATION 0x49 |
sweilz | 0:e4ba03a21caa | 62 | |
sweilz | 0:e4ba03a21caa | 63 | //------------------------------------------------------------------------------------------------------------------------------- |
sweilz | 0:e4ba03a21caa | 64 | // Instruction commands Set |
sweilz | 0:e4ba03a21caa | 65 | //------------------------------------------------------------------------------------------------------------------------------- |
sweilz | 0:e4ba03a21caa | 66 | #define COMMAND_PING 0x01 |
sweilz | 0:e4ba03a21caa | 67 | #define COMMAND_READ_DATA 0x02 |
sweilz | 0:e4ba03a21caa | 68 | #define COMMAND_WRITE_DATA 0x03 |
sweilz | 0:e4ba03a21caa | 69 | #define COMMAND_REG_WRITE_DATA 0x04 |
sweilz | 0:e4ba03a21caa | 70 | #define COMMAND_ACTION 0x05 |
sweilz | 0:e4ba03a21caa | 71 | #define COMMAND_RESET 0x06 |
sweilz | 0:e4ba03a21caa | 72 | #define COMMAND_SYNC_WRITE 0x83 |
sweilz | 0:e4ba03a21caa | 73 | |
sweilz | 0:e4ba03a21caa | 74 | //------------------------------------------------------------------------------------------------------------------------------- |
sweilz | 0:e4ba03a21caa | 75 | //Instruction packet lengths |
sweilz | 0:e4ba03a21caa | 76 | #define READ_ONE_BYTE_LENGTH 0x01 |
sweilz | 0:e4ba03a21caa | 77 | #define READ_TWO_BYTE_LENGTH 0x02 |
sweilz | 0:e4ba03a21caa | 78 | #define RESET_LENGTH 0x02 |
sweilz | 0:e4ba03a21caa | 79 | #define PING_LENGTH 0x02 |
sweilz | 0:e4ba03a21caa | 80 | #define ACTION_LENGTH 0x02 |
sweilz | 0:e4ba03a21caa | 81 | #define SET_ID_LENGTH 0x04 |
sweilz | 0:e4ba03a21caa | 82 | #define SET_BD_LENGTH 0x04 |
sweilz | 0:e4ba03a21caa | 83 | #define SET_RETURN_LEVEL_LENGTH 0x04 |
sweilz | 0:e4ba03a21caa | 84 | #define READ_TEMP_LENGTH 0x04 |
sweilz | 0:e4ba03a21caa | 85 | #define READ_POS_LENGTH 0x04 |
sweilz | 0:e4ba03a21caa | 86 | #define READ_LOAD_LENGTH 0x04 |
sweilz | 0:e4ba03a21caa | 87 | #define READ_SPEED_LENGTH 0x04 |
sweilz | 0:e4ba03a21caa | 88 | #define READ_VOLT_LENGTH 0x04 |
sweilz | 0:e4ba03a21caa | 89 | #define READ_REGISTER_LENGTH 0x04 |
sweilz | 0:e4ba03a21caa | 90 | #define READ_MOVING_LENGTH 0x04 |
sweilz | 0:e4ba03a21caa | 91 | #define READ_LOCK_LENGTH 0x04 |
sweilz | 0:e4ba03a21caa | 92 | #define LED_LENGTH 0x04 |
sweilz | 0:e4ba03a21caa | 93 | #define SET_HOLDING_TORQUE_LENGTH 0x04 |
sweilz | 0:e4ba03a21caa | 94 | #define SET_MAX_TORQUE_LENGTH 0x05 |
sweilz | 0:e4ba03a21caa | 95 | #define SET_ALARM_LENGTH 0x04 |
sweilz | 0:e4ba03a21caa | 96 | #define READ_LOAD_LENGTH 0x04 |
sweilz | 0:e4ba03a21caa | 97 | #define SET_RETURN_LENGTH 0x04 |
sweilz | 0:e4ba03a21caa | 98 | #define WHEEL_LENGTH 0x05 |
sweilz | 0:e4ba03a21caa | 99 | #define SERVO_GOAL_LENGTH 0x07 |
sweilz | 0:e4ba03a21caa | 100 | #define SET_MODE_LENGTH 0x07 |
sweilz | 0:e4ba03a21caa | 101 | #define SET_PUNCH_LENGTH 0x04 |
sweilz | 0:e4ba03a21caa | 102 | #define SET_PID_LENGTH 0x06 |
sweilz | 0:e4ba03a21caa | 103 | #define SET_TEMP_LENGTH 0x04 |
sweilz | 0:e4ba03a21caa | 104 | #define SET_VOLT_LENGTH 0x05 |
sweilz | 0:e4ba03a21caa | 105 | #define SYNC_LOAD_LENGTH 0x0D |
sweilz | 0:e4ba03a21caa | 106 | #define SYNC_DATA_LENGTH 0x02 |
sweilz | 0:e4ba03a21caa | 107 | |
sweilz | 0:e4ba03a21caa | 108 | //------------------------------------------------------------------------------------------------------------------------------- |
sweilz | 0:e4ba03a21caa | 109 | // Specials |
sweilz | 0:e4ba03a21caa | 110 | //------------------------------------------------------------------------------------------------------------------------------- |
sweilz | 0:e4ba03a21caa | 111 | |
sweilz | 0:e4ba03a21caa | 112 | #define OFF 0x00 |
sweilz | 0:e4ba03a21caa | 113 | #define ON 0x01 |
sweilz | 0:e4ba03a21caa | 114 | |
sweilz | 0:e4ba03a21caa | 115 | #define SERVO 0x01 |
sweilz | 0:e4ba03a21caa | 116 | #define WHEEL 0x00 |
sweilz | 0:e4ba03a21caa | 117 | |
sweilz | 0:e4ba03a21caa | 118 | #define LEFT 0x00 |
sweilz | 0:e4ba03a21caa | 119 | #define RIGHT 0x01 |
sweilz | 0:e4ba03a21caa | 120 | |
sweilz | 0:e4ba03a21caa | 121 | #define NONE 0x00 |
sweilz | 0:e4ba03a21caa | 122 | #define READ 0x01 |
sweilz | 0:e4ba03a21caa | 123 | #define ALL 0x02 |
sweilz | 0:e4ba03a21caa | 124 | |
sweilz | 0:e4ba03a21caa | 125 | #define BROADCAST_ID 0xFE |
sweilz | 0:e4ba03a21caa | 126 | |
sweilz | 0:e4ba03a21caa | 127 | #define HEADER 0xFF |
sweilz | 0:e4ba03a21caa | 128 | |
sweilz | 0:e4ba03a21caa | 129 | #define STATUS_PACKET_TIMEOUT 50 // in millis() |
sweilz | 0:e4ba03a21caa | 130 | #define STATUS_FRAME_BUFFER 5 |
sweilz | 0:e4ba03a21caa | 131 | |
sweilz | 0:e4ba03a21caa | 132 | class Dynamixel |
sweilz | 0:e4ba03a21caa | 133 | { |
sweilz | 0:e4ba03a21caa | 134 | private: |
sweilz | 0:e4ba03a21caa | 135 | DigitalOut *dynamixelDi; |
sweilz | 0:e4ba03a21caa | 136 | Serial *dynamixelSerial; |
sweilz | 0:e4ba03a21caa | 137 | void writePacket(void); |
sweilz | 0:e4ba03a21caa | 138 | unsigned int readPacket(void); |
sweilz | 0:e4ba03a21caa | 139 | |
sweilz | 0:e4ba03a21caa | 140 | |
sweilz | 0:e4ba03a21caa | 141 | public: |
sweilz | 0:e4ba03a21caa | 142 | Dynamixel(PinName tx, PinName rx, int baud, PinName di); |
sweilz | 0:e4ba03a21caa | 143 | ~Dynamixel(void); |
sweilz | 0:e4ba03a21caa | 144 | |
sweilz | 0:e4ba03a21caa | 145 | void setMode(uint8_t ID, uint8_t Mode, uint16_t CW_limit = 0, uint16_t CCW_limit = 0); |
sweilz | 0:e4ba03a21caa | 146 | |
sweilz | 0:e4ba03a21caa | 147 | void setLed(uint8_t ID, bool State); |
sweilz | 0:e4ba03a21caa | 148 | void setPosition(uint8_t ID, uint16_t Position, uint16_t Speed); |
sweilz | 0:e4ba03a21caa | 149 | void setWheelSpeed(uint8_t ID, bool Direction, uint16_t Speed); |
sweilz | 0:e4ba03a21caa | 150 | void setWheel3Speed(uint8_t ID[], bool Direction[], uint16_t Speed[]); |
sweilz | 0:e4ba03a21caa | 151 | |
sweilz | 0:e4ba03a21caa | 152 | }; |