LDSC_Robotics_TAs / Mbed 2 deprecated MX28_Sensor_Correction_upgrade

Sensor correction

Dependencies:   LSM9DS0 mbed

Fork of MX28_Sensor_Correction by Chris LU

Mx28.h@2:bd98f7a4e231, 2017-02-13 (annotated)

Committer:
cpul5338
Date:
Mon Feb 13 14:35:01 2017 +0000
Revision:
2:bd98f7a4e231
Parent:
1:3643ef2599cd 
update

Who changed what in which revision?

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