PSL_2021 / servomotor_MX12_Lorenzo

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependents:   PSL_ROBOT_lorenzo robot_lorenzo recepteur_mbed_os_6

MX12.h@5:0cf54586a4be, 2021-11-04 (annotated)

Committer:
denis2nis
Date:
Thu Nov 04 11:48:53 2021 +0000
Revision:
5:0cf54586a4be
Parent:
4:277e5a4cba2e
Child:
6:177aead4eb6b
Include last change of Titouan (Switch to state machine for more reliable parsing); https://os.mbed.com/users/tsoul/code/MX12//rev/4bdd101ce4ec

Who changed what in which revision?

UserRevisionLine numberNew contents of line
denis2nis 1:a9ba9cf928fe 1 /**
denis2nis 1:a9ba9cf928fe 2 * @file MX12.h
denis2nis 1:a9ba9cf928fe 3 * @brief this header file will contain all required
denis2nis 1:a9ba9cf928fe 4 * definitions and basic utilities functions.
denis2nis 2:02f3323a107d 5 * @details comming soon
denis2nis 1:a9ba9cf928fe 6 *
denis2nis 1:a9ba9cf928fe 7 *
denis2nis 1:a9ba9cf928fe 8 */
denis2nis 5:0cf54586a4be 9 #ifndef MBED_MX12_H
denis2nis 5:0cf54586a4be 10 #define MBED_MX12_H
denis2nis 0:7556356a8bcd 11
denis2nis 0:7556356a8bcd 12 #include "mbed.h"
denis2nis 0:7556356a8bcd 13
denis2nis 5:0cf54586a4be 14 #define MX12_DATA_MAX_SIZE 256
denis2nis 5:0cf54586a4be 15 #define MX12_MAX_MOTOR_COUNT 16
denis2nis 5:0cf54586a4be 16
denis2nis 0:7556356a8bcd 17
denis2nis 2:02f3323a107d 18 /* Dynamixel protocol v1.0 : Instructions
denis2nis 2:02f3323a107d 19 ******************************************/
denis2nis 5:0cf54586a4be 20 /** \def PROTOCOL_INSTRUCTION_PING
denis2nis 5:0cf54586a4be 21 Instruction that checks whether the Packet has arrived to a device
denis2nis 5:0cf54586a4be 22 with the same ID as Packet ID
denis2nis 5:0cf54586a4be 23 */
denis2nis 2:02f3323a107d 24 #define PROTOCOL_INSTRUCTION_PING 0x01
denis2nis 2:02f3323a107d 25 #define PROTOCOL_INSTRUCTION_READ 0x02
denis2nis 2:02f3323a107d 26 #define PROTOCOL_INSTRUCTION_WRITE 0x03
denis2nis 2:02f3323a107d 27 #define PROTOCOL_INSTRUCTION_REG_WRITE 0x04
denis2nis 2:02f3323a107d 28 #define PROTOCOL_INSTRUCTION_ACTION 0x05
denis2nis 2:02f3323a107d 29 #define PROTOCOL_INSTRUCTION_FACTORY_RESET 0x06
denis2nis 2:02f3323a107d 30 #define PROTOCOL_INSTRUCTION_REBOOT 0x08
denis2nis 2:02f3323a107d 31 #define PROTOCOL_INSTRUCTION_SYNC_WRITE 0x83
denis2nis 2:02f3323a107d 32 #define PROTOCOL_INSTRUCTION_BULK_READ 0x92
denis2nis 2:02f3323a107d 33
denis2nis 5:0cf54586a4be 34 /* Dynamixel protocol v1.0 : Contro table content
denis2nis 5:0cf54586a4be 35 ************************************************/
denis2nis 5:0cf54586a4be 36 #define CONTROL_TABLE_MODEL_NUMBER 0
denis2nis 5:0cf54586a4be 37 #define CONTROL_TABLE_FIRMWARE_VERSION 2
denis2nis 5:0cf54586a4be 38 #define CONTROL_TABLE_ID 3
denis2nis 5:0cf54586a4be 39 #define CONTROL_TABLE_BAUD_RATE 4
denis2nis 5:0cf54586a4be 40 #define CONTROL_TABLE_RETURN_DELAY_TIME 5
denis2nis 5:0cf54586a4be 41 #define CONTROL_TABLE_CW_ANGLE_LIMIT 6
denis2nis 5:0cf54586a4be 42 #define CONTROL_TABLE_CCW_ANGLE_LIMIT 8
denis2nis 5:0cf54586a4be 43 #define CONTROL_TABLE_TEMPERATURE_LIMIT 11
denis2nis 5:0cf54586a4be 44 #define CONTROL_TABLE_MIN_VOLTAGE_LIMIT 12
denis2nis 5:0cf54586a4be 45 #define CONTROL_TABLE_MAX_VOLTAGE_LIMIT 13
denis2nis 5:0cf54586a4be 46 #define CONTROL_TABLE_MAX_TORQUE 14
denis2nis 5:0cf54586a4be 47 #define CONTROL_TABLE_STATUS_RETURN_LEVEL 16
denis2nis 5:0cf54586a4be 48 #define CONTROL_TABLE_ALARM_LED 17
denis2nis 5:0cf54586a4be 49 #define CONTROL_TABLE_SHUTDOWN 18
denis2nis 5:0cf54586a4be 50 #define CONTROL_TABLE_MULTITURN_OFFSET 20
denis2nis 5:0cf54586a4be 51 #define CONTROL_TABLE_RESOLUTION_DIVIDER 22
denis2nis 5:0cf54586a4be 52 #define CONTROL_TABLE_TORQUE_ENABLE 24
denis2nis 5:0cf54586a4be 53 #define CONTROL_TABLE_LED 25
denis2nis 5:0cf54586a4be 54 #define CONTROL_TABLE_P_GAIN 26
denis2nis 5:0cf54586a4be 55 #define CONTROL_TABLE_I_GAIN 27
denis2nis 5:0cf54586a4be 56 #define CONTROL_TABLE_D_GAIN 28
denis2nis 5:0cf54586a4be 57 #define CONTROL_TABLE_GOAL_POSITION 30
denis2nis 5:0cf54586a4be 58 #define CONTROL_TABLE_MOVING_SPEED 32
denis2nis 5:0cf54586a4be 59 #define CONTROL_TABLE_TORQUE_LIMIT 34
denis2nis 5:0cf54586a4be 60 #define CONTROL_TABLE_PRESENT_POSITION 36
denis2nis 5:0cf54586a4be 61 #define CONTROL_TABLE_PRESENT_SPEED 38
denis2nis 5:0cf54586a4be 62 #define CONTROL_TABLE_PRESENT_LOAD 40
denis2nis 5:0cf54586a4be 63 #define CONTROL_TABLE_PRESENT_VOLTAGE 42
denis2nis 5:0cf54586a4be 64 #define CONTROL_TABLE_PRESENT_TEMPERATURE 43
denis2nis 5:0cf54586a4be 65 #define CONTROL_TABLE_REGISTRED_INSTRUCTION 44
denis2nis 5:0cf54586a4be 66 #define CONTROL_TABLE_MOVING 46
denis2nis 5:0cf54586a4be 67 #define CONTROL_TABLE_LOCK 47
denis2nis 5:0cf54586a4be 68 #define CONTROL_TABLE_PUNCH 48
denis2nis 5:0cf54586a4be 69 #define CONTROL_TABLE_GOAL_ACCELERATION 73
denis2nis 2:02f3323a107d 70
denis2nis 2:02f3323a107d 71 /**
denis2nis 2:02f3323a107d 72 * @brief Class to communicate with Dynamixel MX12 servomotors.
denis2nis 1:a9ba9cf928fe 73 *
denis2nis 2:02f3323a107d 74 * @details
denis2nis 2:02f3323a107d 75 * The servomotors are daisy chained to a serial link of the target
denis2nis 2:02f3323a107d 76 * microcontroller. The class ensures the initialization of serial link
denis2nis 2:02f3323a107d 77 * and the management of communications.
denis2nis 2:02f3323a107d 78 * Transmission of messages to the servomotors is blocking while
denis2nis 2:02f3323a107d 79 * reception is non-blocking thanks to the use of an interrupt routine.
denis2nis 5:0cf54586a4be 80 *
denis2nis 5:0cf54586a4be 81 *
denis2nis 5:0cf54586a4be 82 * @author Titouan Soulard (creator and maintainer until April 2021)
denis2nis 5:0cf54586a4be 83 * @author Bruno Denis (Doxygen documentation and code rearrangement)
denis2nis 1:a9ba9cf928fe 84 *
denis2nis 5:0cf54586a4be 85 * @see Control table of Dynamixel MX12 servomotor
denis2nis 5:0cf54586a4be 86 * @see https://emanual.robotis.com/docs/en/dxl/mx/mx-12w/
denis2nis 5:0cf54586a4be 87 * @see Dynamixel protocol v1.0 manual
denis2nis 5:0cf54586a4be 88 * @see https://emanual.robotis.com/docs/en/dxl/protocol1/
denis2nis 5:0cf54586a4be 89 *
denis2nis 5:0cf54586a4be 90 * @warning
denis2nis 5:0cf54586a4be 91 * enum type is not suitable for the errors status because
denis2nis 5:0cf54586a4be 92 * several errors can be repported simultaneously (B. Denis 11/2021)
denis2nis 1:a9ba9cf928fe 93 */
denis2nis 0:7556356a8bcd 94 class MX12
denis2nis 0:7556356a8bcd 95 {
denis2nis 0:7556356a8bcd 96 public:
denis2nis 0:7556356a8bcd 97
denis2nis 3:add8b050eb86 98 /** Error status occurred during the operation of servomotor.
denis2nis 1:a9ba9cf928fe 99 *
denis2nis 5:0cf54586a4be 100 * @warning
denis2nis 5:0cf54586a4be 101 * enum type is not suitable for the errors status because
denis2nis 5:0cf54586a4be 102 * several errors can be repported simultaneously (B. Denis 11/2021)
denis2nis 1:a9ba9cf928fe 103 */
denis2nis 0:7556356a8bcd 104 enum Status {
denis2nis 5:0cf54586a4be 105 InstructionError, /**< In case of sending an undefined instruction
denis2nis 5:0cf54586a4be 106 or delivering the action instruction
denis2nis 5:0cf54586a4be 107 without the Reg Write instruction */
denis2nis 5:0cf54586a4be 108 OverloadError, /**< When the current load cannot be controlled
denis2nis 5:0cf54586a4be 109 by the set Torque */
denis2nis 5:0cf54586a4be 110 ChecksumError, /**< When the Checksum of the transmitted
denis2nis 5:0cf54586a4be 111 Instruction Packet is incorrect */
denis2nis 5:0cf54586a4be 112 RangeError, /**< When an instruction is out of the range
denis2nis 5:0cf54586a4be 113 for use */
denis2nis 5:0cf54586a4be 114 OverheatingError, /**< When internal temperature of servomotor
denis2nis 5:0cf54586a4be 115 is out of the range of operating
denis2nis 5:0cf54586a4be 116 temperature set in the Control table */
denis2nis 5:0cf54586a4be 117 AngleLimitError, /**< When Goal Position is written out of the
denis2nis 5:0cf54586a4be 118 range from CW Angle Limit to CCW Angle
denis2nis 5:0cf54586a4be 119 Limit */
denis2nis 5:0cf54586a4be 120 InputVoltageError, /**< When the applied voltage is out of the
denis2nis 5:0cf54586a4be 121 range of operating voltage set in the
denis2nis 5:0cf54586a4be 122 Control table */
denis2nis 5:0cf54586a4be 123 Unknown, /**< Combination of several errors (Caution:
denis2nis 5:0cf54586a4be 124 limit of enum approach to code errors) */
denis2nis 3:add8b050eb86 125 Ok ///< no error
denis2nis 0:7556356a8bcd 126 };
denis2nis 0:7556356a8bcd 127
denis2nis 5:0cf54586a4be 128 /** State of packet parser to determine the meaning of reading byte
denis2nis 5:0cf54586a4be 129 * into the received packet (satuts packet) according the Dynamixel
denis2nis 5:0cf54586a4be 130 * protocol v1.0
denis2nis 5:0cf54586a4be 131 */
denis2nis 5:0cf54586a4be 132 enum ParsingState {
denis2nis 5:0cf54586a4be 133 Header, ///< reading the two heading bytes
denis2nis 5:0cf54586a4be 134 Id, ///< reading the servomotor ID byte
denis2nis 5:0cf54586a4be 135 Length, ///< reading byte length of the instruction
denis2nis 5:0cf54586a4be 136 Data, ///< reading parameters
denis2nis 5:0cf54586a4be 137 Checksum, ///< reading one bytes checksum
denis2nis 5:0cf54586a4be 138 };
denis2nis 5:0cf54586a4be 139
denis2nis 5:0cf54586a4be 140 /** Enumeration of states of the acces methode of the master-slave
denis2nis 5:0cf54586a4be 141 * protocol of the communication between the robot controller (master)
denis2nis 5:0cf54586a4be 142 * and the servomotors (slaves).
denis2nis 1:a9ba9cf928fe 143 */
denis2nis 5:0cf54586a4be 144 enum SerialState {
denis2nis 5:0cf54586a4be 145 Writing, /**< Robot controller send an "instruction packet"
denis2nis 5:0cf54586a4be 146 (request) to a servomotor */
denis2nis 5:0cf54586a4be 147 Reading, /**< Robot controller receive a "status packet" from a
denis2nis 5:0cf54586a4be 148 requested servomotor */
denis2nis 5:0cf54586a4be 149 Idle, ///< Robot controller ready for a new cycle request-answer
denis2nis 0:7556356a8bcd 150 };
denis2nis 5:0cf54586a4be 151
denis2nis 5:0cf54586a4be 152 /** type
denis2nis 5:0cf54586a4be 153 */
denis2nis 5:0cf54586a4be 154 struct Frame {
denis2nis 5:0cf54586a4be 155 unsigned char motorId;
denis2nis 5:0cf54586a4be 156 unsigned char length;
denis2nis 5:0cf54586a4be 157 unsigned char data[MX12_DATA_MAX_SIZE];
denis2nis 5:0cf54586a4be 158 unsigned char valid;
denis2nis 5:0cf54586a4be 159 };
denis2nis 5:0cf54586a4be 160
denis2nis 5:0cf54586a4be 161 /**
denis2nis 5:0cf54586a4be 162 */
denis2nis 5:0cf54586a4be 163 struct StateContext {
denis2nis 5:0cf54586a4be 164 unsigned char headingCount;
denis2nis 5:0cf54586a4be 165 unsigned char dataCount;
denis2nis 5:0cf54586a4be 166 unsigned char checksum;
denis2nis 5:0cf54586a4be 167 };
denis2nis 5:0cf54586a4be 168
denis2nis 0:7556356a8bcd 169 /** Create MX12 instance
denis2nis 0:7556356a8bcd 170 *
denis2nis 0:7556356a8bcd 171 * @param tx board pin used for transmission in UART daisy chain link
denis2nis 0:7556356a8bcd 172 * to servomotors
denis2nis 0:7556356a8bcd 173 * @param rx board pin used for reception in UART daisy chain link
denis2nis 0:7556356a8bcd 174 * to servomotors
denis2nis 0:7556356a8bcd 175 * @param baud modulation rate of UART signal, unit: Baud
denis2nis 0:7556356a8bcd 176 * to servomotors
denis2nis 0:7556356a8bcd 177 */
denis2nis 0:7556356a8bcd 178 MX12(PinName tx, PinName rx, int baud=115200);
denis2nis 0:7556356a8bcd 179
denis2nis 0:7556356a8bcd 180 /** Send desired speed to a specifc servomotor
denis2nis 0:7556356a8bcd 181 *
denis2nis 0:7556356a8bcd 182 * @param mot_id a unique value in the daisy chain network to identify
denis2nis 0:7556356a8bcd 183 * each servomotor
denis2nis 0:7556356a8bcd 184 * @param speed a float between -1 and 1 that represents the percentage
denis2nis 0:7556356a8bcd 185 * of maximum requested speed
denis2nis 0:7556356a8bcd 186 */
denis2nis 0:7556356a8bcd 187 void SetSpeed(unsigned char mot_id, float speed);
denis2nis 0:7556356a8bcd 188
denis2nis 0:7556356a8bcd 189 char IsAvailable(void);
denis2nis 0:7556356a8bcd 190
denis2nis 2:02f3323a107d 191 /**
denis2nis 2:02f3323a107d 192 * Build and send an instruction packet to a servomotor according
denis2nis 2:02f3323a107d 193 * DYNAMIXEL Protocol 1.0 (online protocol documentation
denis2nis 2:02f3323a107d 194 * https://emanual.robotis.com/docs/en/dxl/protocol1/)
denis2nis 2:02f3323a107d 195 *
denis2nis 2:02f3323a107d 196 * @param[in] mot_id indicates the ID of the device (servomotor) that
denis2nis 2:02f3323a107d 197 * should receive the Instruction Packet and process it
denis2nis 2:02f3323a107d 198 * @param[in] address data address in the "Control Table of RAM Area"
denis2nis 2:02f3323a107d 199 * of a servomotor MX12 (https://emanual.robotis.com/docs/en/dxl/mx/mx-12w/#control-table-of-ram-area).
denis2nis 2:02f3323a107d 200 * @param[in] len if it is a write instruction, size in bytes
denis2nis 2:02f3323a107d 201 * of the data to write in the "Control Table of RAM Area"
denis2nis 3:add8b050eb86 202 * @param[in, out] data array of char containing parameter of Danamixel
denis2nis 2:02f3323a107d 203 * protocol that are the instruction’s auxiliary data field
denis2nis 2:02f3323a107d 204 *
denis2nis 3:add8b050eb86 205 * @see https://emanual.robotis.com/docs/en/dxl/protocol1/
denis2nis 3:add8b050eb86 206 *
denis2nis 3:add8b050eb86 207 * Packet sent
denis2nis 4:277e5a4cba2e 208 * <PRE>
denis2nis 5:0cf54586a4be 209 * |Header1|Header2|Packet ID|Length|Instruction|Param1...ParamN|Checksum|
denis2nis 5:0cf54586a4be 210 * |-------|-------|---------|------|-----------|---------------|--------|
denis2nis 5:0cf54586a4be 211 * | 0xFF | 0xFF |Packet ID|Length|Instruction|Param1...ParamN| CHKSUM |
denis2nis 5:0cf54586a4be 212 * | cmd[0]| cmd[1]| cmd[2] |cmd[3]| cmd[4] |cmd[5]... | |
denis2nis 5:0cf54586a4be 213 * \\__ ___/ \\_ _/ \\__ __/
denis2nis 5:0cf54586a4be 214 * \/ \/ \/
denis2nis 5:0cf54586a4be 215 * mot_id address data
denis2nis 3:add8b050eb86 216 * (len = N-1)
denis2nis 4:277e5a4cba2e 217 * </PRE>
denis2nis 2:02f3323a107d 218 */
denis2nis 2:02f3323a107d 219 void rw(unsigned char mot_id, char address, char len, char *data);
denis2nis 0:7556356a8bcd 220
denis2nis 5:0cf54586a4be 221 /**
denis2nis 5:0cf54586a4be 222 *
denis2nis 5:0cf54586a4be 223 * @warning
denis2nis 5:0cf54586a4be 224 *
denis2nis 5:0cf54586a4be 225 */
denis2nis 5:0cf54586a4be 226 void PrintSerial();
denis2nis 5:0cf54586a4be 227
denis2nis 5:0cf54586a4be 228 /**
denis2nis 5:0cf54586a4be 229 * Get information from de Error byte of the last status packet
denis2nis 5:0cf54586a4be 230 * received from a servomotor
denis2nis 5:0cf54586a4be 231 *
denis2nis 5:0cf54586a4be 232 * @return
denis2nis 5:0cf54586a4be 233 * One of enum MX12::Status value to describe errors. 'Ok' is
denis2nis 5:0cf54586a4be 234 * returns if no error occurred during the last operation of
denis2nis 5:0cf54586a4be 235 * servomotor.
denis2nis 5:0cf54586a4be 236 *
denis2nis 5:0cf54586a4be 237 * @warning
denis2nis 5:0cf54586a4be 238 * If a combination of several errors is reported the function
denis2nis 5:0cf54586a4be 239 * returns 'Unknown' (B. Denis 11/2021)
denis2nis 5:0cf54586a4be 240 */
denis2nis 5:0cf54586a4be 241 MX12::Status GetStatus(void);
denis2nis 5:0cf54586a4be 242
denis2nis 5:0cf54586a4be 243 /* function aivailaible in a previous version of the class
denis2nis 5:0cf54586a4be 244 */
denis2nis 5:0cf54586a4be 245 // MX12::Status GetStatus(void);
denis2nis 5:0cf54586a4be 246 // void ReadPosition(unsigned char mot_id);
denis2nis 5:0cf54586a4be 247 // float GetPosition(unsigned char mot_id);
denis2nis 5:0cf54586a4be 248
denis2nis 5:0cf54586a4be 249 private:
denis2nis 5:0cf54586a4be 250
denis2nis 5:0cf54586a4be 251 UnbufferedSerial _mx12;
denis2nis 5:0cf54586a4be 252 MX12::ParsingState _pstate;
denis2nis 5:0cf54586a4be 253 MX12::SerialState _sstate;
denis2nis 5:0cf54586a4be 254 MX12::StateContext _scontext;
denis2nis 5:0cf54586a4be 255 MX12::Frame _current_frame;
denis2nis 5:0cf54586a4be 256
denis2nis 5:0cf54586a4be 257 unsigned char _answer;
denis2nis 5:0cf54586a4be 258 unsigned char _stored_frame[MX12_DATA_MAX_SIZE];
denis2nis 5:0cf54586a4be 259 unsigned char _frame_pointer;
denis2nis 5:0cf54586a4be 260
denis2nis 1:a9ba9cf928fe 261 /** Interupt Routine to read in data from UART daisy chain link
denis2nis 1:a9ba9cf928fe 262 * (serial port)
denis2nis 1:a9ba9cf928fe 263 *
denis2nis 1:a9ba9cf928fe 264 */
denis2nis 0:7556356a8bcd 265 void _ReadCallback();
denis2nis 0:7556356a8bcd 266 };
denis2nis 0:7556356a8bcd 267
denis2nis 0:7556356a8bcd 268 #endif /* MBED_MX12_H */