Object for MX28 with rs485

Committer:
gert_lauritsen
Date:
Wed Sep 10 13:06:35 2014 +0000
Revision:
1:c866ce96ceb3
Parent:
0:45d73665e91f
Corrected error in data in function

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
gert_lauritsen 0:45d73665e91f 72
gert_lauritsen 0:45d73665e91f 73 //-------------------------------------------------------------------------------------------------------------------------------
gert_lauritsen 0:45d73665e91f 74 // Instruction commands Set
gert_lauritsen 0:45d73665e91f 75 //-------------------------------------------------------------------------------------------------------------------------------
gert_lauritsen 0:45d73665e91f 76 #define COMMAND_PING 0x01
gert_lauritsen 0:45d73665e91f 77 #define COMMAND_READ_DATA 0x02
gert_lauritsen 0:45d73665e91f 78 #define COMMAND_WRITE_DATA 0x03
gert_lauritsen 0:45d73665e91f 79 #define COMMAND_REG_WRITE_DATA 0x04
gert_lauritsen 0:45d73665e91f 80 #define COMMAND_ACTION 0x05
gert_lauritsen 0:45d73665e91f 81 #define COMMAND_RESET 0x06
gert_lauritsen 0:45d73665e91f 82 #define COMMAND_SYNC_WRITE 0x83
gert_lauritsen 0:45d73665e91f 83
gert_lauritsen 0:45d73665e91f 84
gert_lauritsen 0:45d73665e91f 85 //-------------------------------------------------------------------------------------------------------------------------------
gert_lauritsen 0:45d73665e91f 86 //Instruction packet lengths
gert_lauritsen 0:45d73665e91f 87 #define READ_ONE_BYTE_LENGTH 0x01
gert_lauritsen 0:45d73665e91f 88 #define READ_TWO_BYTE_LENGTH 0x02
gert_lauritsen 0:45d73665e91f 89 #define RESET_LENGTH 0x02
gert_lauritsen 0:45d73665e91f 90 #define PING_LENGTH 0x02
gert_lauritsen 0:45d73665e91f 91 #define ACTION_LENGTH 0x02
gert_lauritsen 0:45d73665e91f 92 #define SET_ID_LENGTH 0x04
gert_lauritsen 0:45d73665e91f 93 #define SET_BD_LENGTH 0x04
gert_lauritsen 0:45d73665e91f 94 #define SET_RETURN_LEVEL_LENGTH 0x04
gert_lauritsen 0:45d73665e91f 95 #define READ_TEMP_LENGTH 0x04
gert_lauritsen 0:45d73665e91f 96 #define READ_POS_LENGTH 0x04
gert_lauritsen 0:45d73665e91f 97 #define READ_LOAD_LENGTH 0x04
gert_lauritsen 0:45d73665e91f 98 #define READ_SPEED_LENGTH 0x04
gert_lauritsen 0:45d73665e91f 99 #define READ_VOLT_LENGTH 0x04
gert_lauritsen 0:45d73665e91f 100 #define READ_REGISTER_LENGTH 0x04
gert_lauritsen 0:45d73665e91f 101 #define READ_MOVING_LENGTH 0x04
gert_lauritsen 0:45d73665e91f 102 #define READ_LOCK_LENGTH 0x04
gert_lauritsen 0:45d73665e91f 103 #define LED_LENGTH 0x04
gert_lauritsen 0:45d73665e91f 104 #define SET_HOLDING_TORQUE_LENGTH 0x04
gert_lauritsen 0:45d73665e91f 105 #define SET_MAX_TORQUE_LENGTH 0x05
gert_lauritsen 0:45d73665e91f 106 #define SET_ALARM_LENGTH 0x04
gert_lauritsen 0:45d73665e91f 107 #define READ_LOAD_LENGTH 0x04
gert_lauritsen 0:45d73665e91f 108 #define SET_RETURN_LENGTH 0x04
gert_lauritsen 0:45d73665e91f 109 #define WHEEL_LENGTH 0x05
gert_lauritsen 0:45d73665e91f 110 #define SERVO_GOAL_LENGTH 0x07
gert_lauritsen 0:45d73665e91f 111 #define SET_MODE_LENGTH 0x07
gert_lauritsen 0:45d73665e91f 112 #define SET_PUNCH_LENGTH 0x04
gert_lauritsen 0:45d73665e91f 113 #define SET_PID_LENGTH 0x06
gert_lauritsen 0:45d73665e91f 114 #define SET_TEMP_LENGTH 0x04
gert_lauritsen 0:45d73665e91f 115 #define SET_VOLT_LENGTH 0x05
gert_lauritsen 0:45d73665e91f 116 #define SYNC_LOAD_LENGTH 0x0D
gert_lauritsen 0:45d73665e91f 117 #define SYNC_DATA_LENGTH 0x02
gert_lauritsen 0:45d73665e91f 118
gert_lauritsen 0:45d73665e91f 119
gert_lauritsen 0:45d73665e91f 120 //-------------------------------------------------------------------------------------------------------------------------------
gert_lauritsen 0:45d73665e91f 121 // Specials
gert_lauritsen 0:45d73665e91f 122 //-------------------------------------------------------------------------------------------------------------------------------
gert_lauritsen 0:45d73665e91f 123
gert_lauritsen 0:45d73665e91f 124 #define OFF 0x00
gert_lauritsen 0:45d73665e91f 125 #define ON 0x01
gert_lauritsen 0:45d73665e91f 126
gert_lauritsen 0:45d73665e91f 127 #define SERVO 0x01
gert_lauritsen 0:45d73665e91f 128 #define WHEEL 0x00
gert_lauritsen 0:45d73665e91f 129
gert_lauritsen 0:45d73665e91f 130 #define LEFT 0x00
gert_lauritsen 0:45d73665e91f 131 #define RIGHT 0x01
gert_lauritsen 0:45d73665e91f 132
gert_lauritsen 0:45d73665e91f 133 #define NONE 0x00
gert_lauritsen 0:45d73665e91f 134 #define READ 0x01
gert_lauritsen 0:45d73665e91f 135 #define ALL 0x02
gert_lauritsen 0:45d73665e91f 136
gert_lauritsen 0:45d73665e91f 137 #define BROADCAST_ID 0xFE
gert_lauritsen 0:45d73665e91f 138
gert_lauritsen 0:45d73665e91f 139 #define HEADER 0xFF
gert_lauritsen 0:45d73665e91f 140
gert_lauritsen 0:45d73665e91f 141 #define STATUS_PACKET_TIMEOUT 50 // in millis()
gert_lauritsen 0:45d73665e91f 142 #define STATUS_FRAME_BUFFER 5
gert_lauritsen 0:45d73665e91f 143
gert_lauritsen 0:45d73665e91f 144
gert_lauritsen 0:45d73665e91f 145
gert_lauritsen 0:45d73665e91f 146 class DynamixelClass {
gert_lauritsen 0:45d73665e91f 147 private:
gert_lauritsen 0:45d73665e91f 148 DigitalOut *servoSerialDir;
gert_lauritsen 0:45d73665e91f 149 Serial *servoSerial;
gert_lauritsen 0:45d73665e91f 150 void transmitInstructionPacket(void);
gert_lauritsen 0:45d73665e91f 151 unsigned int readStatusPacket(void);
gert_lauritsen 0:45d73665e91f 152 bool overflow;
gert_lauritsen 1:c866ce96ceb3 153 void debugframe(void);
gert_lauritsen 1:c866ce96ceb3 154 void debugStatusframe(void);
gert_lauritsen 1:c866ce96ceb3 155 DigitalOut *led2;
gert_lauritsen 0:45d73665e91f 156 public:
gert_lauritsen 0:45d73665e91f 157
gert_lauritsen 0:45d73665e91f 158 DynamixelClass(int baud,PinName D_Pin,PinName tx, PinName rx); //Constructor
gert_lauritsen 0:45d73665e91f 159 ~DynamixelClass(void); //destruktor
gert_lauritsen 0:45d73665e91f 160
gert_lauritsen 0:45d73665e91f 161 void NewBaudRate(int baud);
gert_lauritsen 0:45d73665e91f 162 unsigned int reset(unsigned char);
gert_lauritsen 0:45d73665e91f 163 unsigned int ping(unsigned char);
gert_lauritsen 0:45d73665e91f 164
gert_lauritsen 0:45d73665e91f 165 unsigned int setStatusPaketReturnDelay(unsigned char,unsigned char);
gert_lauritsen 0:45d73665e91f 166 unsigned int setID(unsigned char, unsigned char);
gert_lauritsen 0:45d73665e91f 167 unsigned int setBaudRate(unsigned char, long);
gert_lauritsen 0:45d73665e91f 168 unsigned int setMaxTorque(unsigned char, int);
gert_lauritsen 0:45d73665e91f 169 unsigned int setHoldingTorque(unsigned char, bool);
gert_lauritsen 0:45d73665e91f 170 unsigned int setAlarmShutdown(unsigned char,unsigned char);
gert_lauritsen 0:45d73665e91f 171 unsigned int setStatusPaket(unsigned char,unsigned char);
gert_lauritsen 0:45d73665e91f 172 unsigned int setMode(unsigned char, bool, unsigned int, unsigned int);
gert_lauritsen 0:45d73665e91f 173 unsigned int setPunch(unsigned char,unsigned int);
gert_lauritsen 0:45d73665e91f 174 unsigned int setPID(unsigned char,unsigned char,unsigned char,unsigned char);
gert_lauritsen 0:45d73665e91f 175 unsigned int setTemp(unsigned char,unsigned char);
gert_lauritsen 0:45d73665e91f 176 unsigned int setVoltage(unsigned char,unsigned char,unsigned char);
gert_lauritsen 0:45d73665e91f 177
gert_lauritsen 0:45d73665e91f 178 unsigned int servo(unsigned char, unsigned int, unsigned int);
gert_lauritsen 0:45d73665e91f 179 unsigned int servoPreload(unsigned char, unsigned int, unsigned int);
gert_lauritsen 0:45d73665e91f 180 unsigned int wheel(unsigned char, bool, unsigned int);
gert_lauritsen 0:45d73665e91f 181 void wheelSync(unsigned char,bool,unsigned int,unsigned char, bool,unsigned int,unsigned char, bool,unsigned int);
gert_lauritsen 0:45d73665e91f 182 unsigned int wheelPreload(unsigned char, bool, unsigned int);
gert_lauritsen 0:45d73665e91f 183
gert_lauritsen 0:45d73665e91f 184 unsigned int action(unsigned char);
gert_lauritsen 0:45d73665e91f 185
gert_lauritsen 0:45d73665e91f 186 unsigned int readTemperature(unsigned char);
gert_lauritsen 0:45d73665e91f 187 unsigned int readVoltage(unsigned char);
gert_lauritsen 0:45d73665e91f 188 unsigned int readPosition(unsigned char);
gert_lauritsen 0:45d73665e91f 189 unsigned int readLoad(unsigned char);
gert_lauritsen 0:45d73665e91f 190 unsigned int readSpeed(unsigned char);
gert_lauritsen 0:45d73665e91f 191
gert_lauritsen 0:45d73665e91f 192 unsigned int checkRegister(unsigned char);
gert_lauritsen 0:45d73665e91f 193 unsigned int checkMovement(unsigned char);
gert_lauritsen 0:45d73665e91f 194 unsigned int checkLock(unsigned char);
gert_lauritsen 0:45d73665e91f 195
gert_lauritsen 0:45d73665e91f 196 unsigned int ledState(unsigned char, bool);
gert_lauritsen 0:45d73665e91f 197 };
gert_lauritsen 0:45d73665e91f 198
gert_lauritsen 0:45d73665e91f 199 #endif