Liews Wuttipat / libz_dynamixel

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?

UserRevisionLine numberNew 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 };