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@24:2ab26ed08c83, 2021-11-13 (annotated)

Committer:
bruno2nis
Date:
Sat Nov 13 10:12:09 2021 +0100
Revision:
24:2ab26ed08c83
Parent:
16:8fcf79e02dd4
Child:
25:bc51575e3304
Add MX12::SetSpeed_rad_s() method

Who changed what in which revision?

UserRevisionLine numberNew contents of line
denis2nis 1:a9ba9cf928fe 1 /**
denis2nis 9:b4a5187fdec6 2 * @file MX12.h
denis2nis 13:ccda9a56fef1 3 * @brief this header file contains all required definitions and
denis2nis 8:e74ef93ae660 4 * basic utilities functions to manage au bus of servomotor
denis2nis 8:e74ef93ae660 5 * Dynaminel MX12
denis2nis 8:e74ef93ae660 6 */
denis2nis 8:e74ef93ae660 7 #ifndef MBED_MX12_H_
denis2nis 8:e74ef93ae660 8 #define MBED_MX12_H_
denis2nis 0:7556356a8bcd 9
denis2nis 0:7556356a8bcd 10 #include "mbed.h"
denis2nis 0:7556356a8bcd 11
denis2nis 13:ccda9a56fef1 12 #define MX12_PACKET_MAX_SIZE 32
denis2nis 5:0cf54586a4be 13 #define MX12_MAX_MOTOR_COUNT 16
denis2nis 5:0cf54586a4be 14
bruno2nis 24:2ab26ed08c83 15 #define MX12_SPEED_QUANTA_RPM 0.916 /* Data from MX12 e-manual */
bruno2nis 24:2ab26ed08c83 16 #define MX12_ABSOLUTE_MAX_SPEED_RAD_S 98.1
bruno2nis 24:2ab26ed08c83 17 /* BOT_SERVO_SPEED_QUANTA_RAD_PER_SEC = BOT_SERVO_SPEED_QUANTA_RPM*2pi / 60s */
bruno2nis 24:2ab26ed08c83 18 #define BOT_SERVO_SPEED_QUANTA_RAD_PER_SEC 0.0959
bruno2nis 24:2ab26ed08c83 19 /* SERVO_FULL_SCALE_SPEED_RAD_PER_SEC = BOT_SERVO_SPEED_QUANTA_RAD_PER_SEC*1024 */
bruno2nis 24:2ab26ed08c83 20 #define SERVO_FULL_SCALE_SPEED_RAD_PER_SEC 98.13
bruno2nis 24:2ab26ed08c83 21
bruno2nis 24:2ab26ed08c83 22
denis2nis 2:02f3323a107d 23 /* Dynamixel protocol v1.0 : Instructions
denis2nis 2:02f3323a107d 24 ******************************************/
denis2nis 2:02f3323a107d 25 #define PROTOCOL_INSTRUCTION_PING 0x01
denis2nis 2:02f3323a107d 26 #define PROTOCOL_INSTRUCTION_READ 0x02
denis2nis 2:02f3323a107d 27 #define PROTOCOL_INSTRUCTION_WRITE 0x03
denis2nis 2:02f3323a107d 28 #define PROTOCOL_INSTRUCTION_REG_WRITE 0x04
denis2nis 2:02f3323a107d 29 #define PROTOCOL_INSTRUCTION_ACTION 0x05
denis2nis 2:02f3323a107d 30 #define PROTOCOL_INSTRUCTION_FACTORY_RESET 0x06
denis2nis 2:02f3323a107d 31 #define PROTOCOL_INSTRUCTION_REBOOT 0x08
denis2nis 2:02f3323a107d 32 #define PROTOCOL_INSTRUCTION_SYNC_WRITE 0x83
denis2nis 2:02f3323a107d 33 #define PROTOCOL_INSTRUCTION_BULK_READ 0x92
denis2nis 2:02f3323a107d 34
denis2nis 5:0cf54586a4be 35 /* Dynamixel protocol v1.0 : Contro table content
denis2nis 5:0cf54586a4be 36 ************************************************/
denis2nis 5:0cf54586a4be 37 #define CONTROL_TABLE_MODEL_NUMBER 0
denis2nis 5:0cf54586a4be 38 #define CONTROL_TABLE_FIRMWARE_VERSION 2
denis2nis 5:0cf54586a4be 39 #define CONTROL_TABLE_ID 3
denis2nis 5:0cf54586a4be 40 #define CONTROL_TABLE_BAUD_RATE 4
denis2nis 5:0cf54586a4be 41 #define CONTROL_TABLE_RETURN_DELAY_TIME 5
denis2nis 5:0cf54586a4be 42 #define CONTROL_TABLE_CW_ANGLE_LIMIT 6
denis2nis 5:0cf54586a4be 43 #define CONTROL_TABLE_CCW_ANGLE_LIMIT 8
denis2nis 5:0cf54586a4be 44 #define CONTROL_TABLE_TEMPERATURE_LIMIT 11
denis2nis 5:0cf54586a4be 45 #define CONTROL_TABLE_MIN_VOLTAGE_LIMIT 12
denis2nis 5:0cf54586a4be 46 #define CONTROL_TABLE_MAX_VOLTAGE_LIMIT 13
denis2nis 5:0cf54586a4be 47 #define CONTROL_TABLE_MAX_TORQUE 14
denis2nis 5:0cf54586a4be 48 #define CONTROL_TABLE_STATUS_RETURN_LEVEL 16
denis2nis 5:0cf54586a4be 49 #define CONTROL_TABLE_ALARM_LED 17
denis2nis 5:0cf54586a4be 50 #define CONTROL_TABLE_SHUTDOWN 18
denis2nis 5:0cf54586a4be 51 #define CONTROL_TABLE_MULTITURN_OFFSET 20
denis2nis 5:0cf54586a4be 52 #define CONTROL_TABLE_RESOLUTION_DIVIDER 22
denis2nis 5:0cf54586a4be 53 #define CONTROL_TABLE_TORQUE_ENABLE 24
denis2nis 5:0cf54586a4be 54 #define CONTROL_TABLE_LED 25
denis2nis 5:0cf54586a4be 55 #define CONTROL_TABLE_P_GAIN 26
denis2nis 5:0cf54586a4be 56 #define CONTROL_TABLE_I_GAIN 27
denis2nis 5:0cf54586a4be 57 #define CONTROL_TABLE_D_GAIN 28
denis2nis 5:0cf54586a4be 58 #define CONTROL_TABLE_GOAL_POSITION 30
denis2nis 5:0cf54586a4be 59 #define CONTROL_TABLE_MOVING_SPEED 32
denis2nis 5:0cf54586a4be 60 #define CONTROL_TABLE_TORQUE_LIMIT 34
denis2nis 5:0cf54586a4be 61 #define CONTROL_TABLE_PRESENT_POSITION 36
denis2nis 5:0cf54586a4be 62 #define CONTROL_TABLE_PRESENT_SPEED 38
denis2nis 5:0cf54586a4be 63 #define CONTROL_TABLE_PRESENT_LOAD 40
denis2nis 5:0cf54586a4be 64 #define CONTROL_TABLE_PRESENT_VOLTAGE 42
denis2nis 5:0cf54586a4be 65 #define CONTROL_TABLE_PRESENT_TEMPERATURE 43
denis2nis 5:0cf54586a4be 66 #define CONTROL_TABLE_REGISTRED_INSTRUCTION 44
denis2nis 5:0cf54586a4be 67 #define CONTROL_TABLE_MOVING 46
denis2nis 5:0cf54586a4be 68 #define CONTROL_TABLE_LOCK 47
denis2nis 5:0cf54586a4be 69 #define CONTROL_TABLE_PUNCH 48
denis2nis 5:0cf54586a4be 70 #define CONTROL_TABLE_GOAL_ACCELERATION 73
denis2nis 2:02f3323a107d 71
denis2nis 2:02f3323a107d 72 /**
denis2nis 8:e74ef93ae660 73 * @brief Class to communicate with Dynamixel MX12 servomotors
denis2nis 1:a9ba9cf928fe 74 *
denis2nis 2:02f3323a107d 75 * @details
denis2nis 8:e74ef93ae660 76 *
denis2nis 16:8fcf79e02dd4 77 * The servomotors are daisy chained to a serial link of the target
denis2nis 16:8fcf79e02dd4 78 * microcontroller. The class ensures the initialization of serial link
denis2nis 16:8fcf79e02dd4 79 * and the management of communications mainly to control rotational
denis2nis 16:8fcf79e02dd4 80 * speed of servomotors.
denis2nis 8:e74ef93ae660 81 *
denis2nis 16:8fcf79e02dd4 82 * Transmission of messages to the servomotors is blocking while
denis2nis 16:8fcf79e02dd4 83 * reception is non-blocking thanks to the use of an interrupt routine.
denis2nis 5:0cf54586a4be 84 *
denis2nis 5:0cf54586a4be 85 * @author Titouan Soulard (creator and maintainer until April 2021)
denis2nis 5:0cf54586a4be 86 * @author Bruno Denis (Doxygen documentation and code rearrangement)
denis2nis 1:a9ba9cf928fe 87 *
denis2nis 5:0cf54586a4be 88 * @see Control table of Dynamixel MX12 servomotor
denis2nis 5:0cf54586a4be 89 * @see https://emanual.robotis.com/docs/en/dxl/mx/mx-12w/
denis2nis 5:0cf54586a4be 90 * @see Dynamixel protocol v1.0 manual
denis2nis 5:0cf54586a4be 91 * @see https://emanual.robotis.com/docs/en/dxl/protocol1/
denis2nis 12:acfd6c46954b 92 *
denis2nis 12:acfd6c46954b 93 * Example of class usage to send two instruction packets to servomotor #1
denis2nis 12:acfd6c46954b 94 * with one second gap. First ask to move at 10% of maximum speed and the
denis2nis 12:acfd6c46954b 95 * stop the movement.
denis2nis 12:acfd6c46954b 96 *
denis2nis 12:acfd6c46954b 97 * @code
denis2nis 12:acfd6c46954b 98 * #include "mbed.h"
denis2nis 12:acfd6c46954b 99 * #include "MX12.h"
denis2nis 12:acfd6c46954b 100 *
denis2nis 12:acfd6c46954b 101 * #define SERVO_TX_PIN PC_4
denis2nis 12:acfd6c46954b 102 * #define SERVO_RX_PIN PC_5
denis2nis 12:acfd6c46954b 103 * #define SERVO_BAUD 115200
denis2nis 12:acfd6c46954b 104 * #define SERVO_ID 1
denis2nis 12:acfd6c46954b 105 *
denis2nis 12:acfd6c46954b 106 * #define DELAY_1000ms 1000
denis2nis 12:acfd6c46954b 107 * #define DELAY_1ms 1
denis2nis 12:acfd6c46954b 108 *
denis2nis 12:acfd6c46954b 109 * MX12 servo_bus(SERVO_TX_PIN, SERVO_RX_PIN, SERVO_BAUD);
denis2nis 12:acfd6c46954b 110 * float relative_speed;
denis2nis 12:acfd6c46954b 111 *
denis2nis 12:acfd6c46954b 112 * int main()
denis2nis 12:acfd6c46954b 113 * {
denis2nis 12:acfd6c46954b 114 * // Set speed of SERVO_ID servomotor to 10%
denis2nis 12:acfd6c46954b 115 * // Send on servo_bus to SERVO_ID motor the instruction "moving speed"
denis2nis 12:acfd6c46954b 116 * // with parameter 0.1 (10% of maximum speed)
denis2nis 12:acfd6c46954b 117 * relative_speed = 0.1;
denis2nis 12:acfd6c46954b 118 * servo_bus.SetSpeed(SERVO_ID, relative_speed);
denis2nis 12:acfd6c46954b 119 *
denis2nis 12:acfd6c46954b 120 * // wait for one second
denis2nis 12:acfd6c46954b 121 * thread_sleep_for(DELAY_1000ms);
denis2nis 12:acfd6c46954b 122 *
denis2nis 12:acfd6c46954b 123 * // set speed of SERVO_ID servomotor to 0%
denis2nis 12:acfd6c46954b 124 * relative_speed = 0.0;
denis2nis 12:acfd6c46954b 125 * servo_bus.SetSpeed(SERVO_ID, relative_speed);
denis2nis 12:acfd6c46954b 126 *
denis2nis 12:acfd6c46954b 127 * // infinite loop
denis2nis 12:acfd6c46954b 128 * while (true) thread_sleep_for(DELAY_1000ms);
denis2nis 12:acfd6c46954b 129 * }
denis2nis 12:acfd6c46954b 130 * @endcode
denis2nis 5:0cf54586a4be 131 *
denis2nis 8:e74ef93ae660 132 * @warning
denis2nis 16:8fcf79e02dd4 133 * Warning: receipt of packets from servomotors has not been tested
denis2nis 16:8fcf79e02dd4 134 * (hardwre problem). However, sending move speed instruction is operational.
denis2nis 8:e74ef93ae660 135 *
denis2nis 16:8fcf79e02dd4 136 * @warning
denis2nis 16:8fcf79e02dd4 137 * Warning: Error field of status packet if decoded by GetStatus() returns
denis2nis 16:8fcf79e02dd4 138 * an enumation type "Status". As several errors can be reported
denis2nis 16:8fcf79e02dd4 139 * simultaneously the type enum is not suitable (B. Denis 11/2021).
denis2nis 10:ca9afe156ee1 140 *
denis2nis 11:9bc7f5e2ccee 141 * @bug
denis2nis 10:ca9afe156ee1 142 * Bug: GetStatus() method has a hazardous behavior because it provides
denis2nis 10:ca9afe156ee1 143 * a result by reading the content of the variable _current_frame
denis2nis 13:ccda9a56fef1 144 * which can be modified at any time by the private ISR _Rx_interrupt().
denis2nis 8:e74ef93ae660 145 *
denis2nis 1:a9ba9cf928fe 146 */
denis2nis 0:7556356a8bcd 147 class MX12
denis2nis 0:7556356a8bcd 148 {
denis2nis 0:7556356a8bcd 149 public:
denis2nis 0:7556356a8bcd 150
denis2nis 8:e74ef93ae660 151 /** Error status occurred during the operation of servomotor.
denis2nis 8:e74ef93ae660 152 *
denis2nis 8:e74ef93ae660 153 * @warning
denis2nis 8:e74ef93ae660 154 * Enumaration type is not suitable for all error status because
denis2nis 8:e74ef93ae660 155 * it can denote only one error at a time (B. Denis 11/2021)
denis2nis 8:e74ef93ae660 156 */
denis2nis 8:e74ef93ae660 157 enum Status {
denis2nis 8:e74ef93ae660 158 InstructionError, /**< In case of sending an undefined instruction
denis2nis 8:e74ef93ae660 159 or delivering the action instruction
denis2nis 8:e74ef93ae660 160 without the Reg Write instruction */
denis2nis 8:e74ef93ae660 161 OverloadError, /**< When the current load cannot be controlled
denis2nis 8:e74ef93ae660 162 by the set Torque */
denis2nis 8:e74ef93ae660 163 ChecksumError, /**< When the Checksum of the transmitted
denis2nis 8:e74ef93ae660 164 Instruction Packet is incorrect */
denis2nis 8:e74ef93ae660 165 RangeError, /**< When an instruction is out of the range
denis2nis 8:e74ef93ae660 166 for use */
denis2nis 8:e74ef93ae660 167 OverheatingError, /**< When internal temperature of servomotor
denis2nis 8:e74ef93ae660 168 is out of the range of operating
denis2nis 8:e74ef93ae660 169 temperature set in the Control table */
denis2nis 8:e74ef93ae660 170 AngleLimitError, /**< When Goal Position is written out of the
denis2nis 8:e74ef93ae660 171 range from CW Angle Limit to CCW Angle
denis2nis 8:e74ef93ae660 172 Limit */
denis2nis 8:e74ef93ae660 173 InputVoltageError, /**< When the applied voltage is out of the
denis2nis 8:e74ef93ae660 174 range of operating voltage set in the
denis2nis 8:e74ef93ae660 175 Control table */
denis2nis 8:e74ef93ae660 176 Unknown, /**< Combination of several errors (Caution:
denis2nis 8:e74ef93ae660 177 limit of enum approach to code errors) */
denis2nis 8:e74ef93ae660 178 Ok ///< no error
denis2nis 8:e74ef93ae660 179 };
denis2nis 8:e74ef93ae660 180
denis2nis 13:ccda9a56fef1 181 /** Enumeration of states of the medium acces control (MAC)
denis2nis 13:ccda9a56fef1 182 * of the master-slave protocol of the communication between
denis2nis 13:ccda9a56fef1 183 * the robot controller (master) and the servomotors (slaves).
denis2nis 8:e74ef93ae660 184 */
bruno2nis 24:2ab26ed08c83 185 enum SerialState {
denis2nis 8:e74ef93ae660 186 Writing, /**< Robot controller send an "instruction packet"
denis2nis 8:e74ef93ae660 187 (request) to a servomotor */
denis2nis 8:e74ef93ae660 188 Reading, /**< Robot controller receive a "status packet" from a
denis2nis 8:e74ef93ae660 189 requested servomotor */
denis2nis 8:e74ef93ae660 190 Idle ///< Robot controller ready for a new cycle request-answer
denis2nis 8:e74ef93ae660 191 };
denis2nis 8:e74ef93ae660 192
denis2nis 8:e74ef93ae660 193 /** Structure of store status packet (also known as return packet)
denis2nis 8:e74ef93ae660 194 * according tne Dynamixel protocol v1.0, which are messages sent
denis2nis 8:e74ef93ae660 195 * by servomotors to contriller in response of an instruction packet
denis2nis 8:e74ef93ae660 196 */
denis2nis 13:ccda9a56fef1 197 struct Status_packet {
denis2nis 13:ccda9a56fef1 198 unsigned char raw[MX12_PACKET_MAX_SIZE]; /**< bytes received */
denis2nis 13:ccda9a56fef1 199 unsigned char n_byte; /**< Number of received byte */
denis2nis 13:ccda9a56fef1 200 unsigned char servo_id; /**< Identifier field denoted servomotor
denis2nis 13:ccda9a56fef1 201 * involved in the message */
denis2nis 13:ccda9a56fef1 202 unsigned char length; /**< Length field of status packet */
denis2nis 13:ccda9a56fef1 203 unsigned char error; /**< Error field of status packet */
denis2nis 13:ccda9a56fef1 204 unsigned char n_param; /**< Number of received parameter*/
denis2nis 13:ccda9a56fef1 205 unsigned char param[MX12_PACKET_MAX_SIZE]; /**< array of received
denis2nis 13:ccda9a56fef1 206 * parameters */
denis2nis 13:ccda9a56fef1 207 unsigned char received_checksum; /**< Received checksum field */
denis2nis 13:ccda9a56fef1 208 unsigned char calculated_checksum; /**< Calculated checksum */
denis2nis 13:ccda9a56fef1 209 bool parsed; /**< status packet is correctly parsed */
denis2nis 13:ccda9a56fef1 210 bool valid; /**< received_checksum == calculated_checksum */
denis2nis 8:e74ef93ae660 211 };
denis2nis 8:e74ef93ae660 212
denis2nis 13:ccda9a56fef1 213 /**
denis2nis 13:ccda9a56fef1 214 * @brief Discrete states of the packet parser.
denis2nis 13:ccda9a56fef1 215 *
denis2nis 13:ccda9a56fef1 216 * @detail
denis2nis 13:ccda9a56fef1 217 * Each value corresponding to a field that the parser
denis2nis 13:ccda9a56fef1 218 * can identify according the Dynamixel protocol v1.0.
denis2nis 13:ccda9a56fef1 219 * Enumeration include field of instruction packets
denis2nis 13:ccda9a56fef1 220 * (packets sent to servomotors) and field of status packets
denis2nis 13:ccda9a56fef1 221 * (packets received from servomotors).
denis2nis 9:b4a5187fdec6 222 */
denis2nis 13:ccda9a56fef1 223 enum PacketField {
denis2nis 13:ccda9a56fef1 224 Header1, ///< Heading1 field of instruction or status packet
denis2nis 13:ccda9a56fef1 225 Header2, ///< Heading2 field of instruction or status packet
denis2nis 13:ccda9a56fef1 226 Id, ///< ID field of instruction or status packet
denis2nis 13:ccda9a56fef1 227 Length, ///< Length field of status packet
denis2nis 13:ccda9a56fef1 228 Error, ///< Error status field of status packet
denis2nis 13:ccda9a56fef1 229 Instruction, ///< Instruction field of instruction packet
denis2nis 13:ccda9a56fef1 230 Data, ///< expect to read parameter fields of status packet
denis2nis 13:ccda9a56fef1 231 Checksum ///< expect to read Checksum field of status packet
denis2nis 9:b4a5187fdec6 232 };
denis2nis 9:b4a5187fdec6 233
denis2nis 13:ccda9a56fef1 234 /** Structure to store the current state of packet parserComplement to the ParsingState enumeration to store the reading
denis2nis 9:b4a5187fdec6 235 * progress of each section of the status packet.
denis2nis 8:e74ef93ae660 236 */
denis2nis 13:ccda9a56fef1 237 struct ParserState {
denis2nis 13:ccda9a56fef1 238 MX12::PacketField expected_field; ///< next expected field
denis2nis 13:ccda9a56fef1 239 unsigned char byte_index; ///< index of byte already parsed
denis2nis 13:ccda9a56fef1 240 unsigned char param_index; ///< index of parameter already parsed
denis2nis 8:e74ef93ae660 241 };
denis2nis 5:0cf54586a4be 242
denis2nis 16:8fcf79e02dd4 243 /** Create an instance of MX12 and attach it to a serial link
denis2nis 8:e74ef93ae660 244 *
denis2nis 8:e74ef93ae660 245 * @param tx board pin used for transmission in UART daisy chain link
denis2nis 8:e74ef93ae660 246 * to servomotors
denis2nis 8:e74ef93ae660 247 * @param rx board pin used for reception in UART daisy chain link
denis2nis 8:e74ef93ae660 248 * to servomotors
denis2nis 8:e74ef93ae660 249 * @param baud modulation rate of UART signal, unit: Baud
denis2nis 8:e74ef93ae660 250 * to servomotors
denis2nis 8:e74ef93ae660 251 */
denis2nis 8:e74ef93ae660 252 MX12(PinName tx, PinName rx, int baud=115200);
denis2nis 8:e74ef93ae660 253
bruno2nis 24:2ab26ed08c83 254 /** Send desired normalized speed to a specific servomotor
denis2nis 8:e74ef93ae660 255 *
denis2nis 8:e74ef93ae660 256 * @param mot_id a unique value in the daisy chain network to identify
denis2nis 8:e74ef93ae660 257 * each servomotor
denis2nis 8:e74ef93ae660 258 * @param speed a float between -1 and 1 that represents the percentage
denis2nis 8:e74ef93ae660 259 * of maximum requested speed
denis2nis 8:e74ef93ae660 260 */
denis2nis 8:e74ef93ae660 261 void SetSpeed(unsigned char mot_id, float speed);
denis2nis 8:e74ef93ae660 262
bruno2nis 24:2ab26ed08c83 263 /** Send desired ablosute speed to a specific servomotor
bruno2nis 24:2ab26ed08c83 264 *
bruno2nis 24:2ab26ed08c83 265 * @param mot_id a unique value in the daisy chain network to identify
bruno2nis 24:2ab26ed08c83 266 * each servomotor
bruno2nis 24:2ab26ed08c83 267 * @param speed desired speed in radians per second
bruno2nis 24:2ab26ed08c83 268 */
bruno2nis 24:2ab26ed08c83 269 void SetSpeed_rad_s(unsigned char mot_id, float speed);
bruno2nis 24:2ab26ed08c83 270
denis2nis 10:ca9afe156ee1 271 /** Informs about the availability of the bus for a new transmission
denis2nis 10:ca9afe156ee1 272 * of instruction packet (Method Access Control)
denis2nis 10:ca9afe156ee1 273 *
denis2nis 10:ca9afe156ee1 274 * @returns true if bus ready for a new transmission, otherwise false
denis2nis 9:b4a5187fdec6 275 */
denis2nis 8:e74ef93ae660 276 char IsAvailable(void);
denis2nis 8:e74ef93ae660 277
denis2nis 8:e74ef93ae660 278 /**
denis2nis 13:ccda9a56fef1 279 * @brief Build and send an instruction packet to a servomotor
denis2nis 13:ccda9a56fef1 280 * according Dynamixel Protocol 1.0
denis2nis 13:ccda9a56fef1 281 *
denis2nis 13:ccda9a56fef1 282 * @detail This method is limited to only two kind of instruction :
denis2nis 13:ccda9a56fef1 283 * read data from a servomoteur et write data on servomotor.
denis2nis 13:ccda9a56fef1 284 * Ping, Reg Write, Action, Factory Reset, Rebbot, Sync Write
denis2nis 13:ccda9a56fef1 285 * and Bulk Read are not supported.
denis2nis 13:ccda9a56fef1 286 *
denis2nis 13:ccda9a56fef1 287 * For the method to issue a read instruction, set the data
denis2nis 13:ccda9a56fef1 288 * parameter to NULL, otherwise the method issue a
denis2nis 13:ccda9a56fef1 289 * write instruction
denis2nis 8:e74ef93ae660 290 *
denis2nis 8:e74ef93ae660 291 * @param[in] mot_id indicates the ID of the device (servomotor) that
denis2nis 8:e74ef93ae660 292 * should receive the Instruction Packet and process it
denis2nis 8:e74ef93ae660 293 * @param[in] address data address in the "Control Table of RAM Area"
denis2nis 8:e74ef93ae660 294 * of a servomotor MX12
denis2nis 8:e74ef93ae660 295 * (https://emanual.robotis.com/docs/en/dxl/mx/mx-12w/#control-table-of-ram-area).
denis2nis 8:e74ef93ae660 296 * @param[in] len if it is a write instruction, size in bytes
denis2nis 8:e74ef93ae660 297 * of the data to write in the "Control Table of RAM Area"
denis2nis 8:e74ef93ae660 298 * @param[in] data array of char containing parameter of Danamixel
denis2nis 13:ccda9a56fef1 299 * protocol that are the instruction’s auxiliary data field.
denis2nis 13:ccda9a56fef1 300 * Set to NULL for read instruction, otherwise the method issue
denis2nis 13:ccda9a56fef1 301 * a write instruction
denis2nis 8:e74ef93ae660 302 *
denis2nis 8:e74ef93ae660 303 * @see https://emanual.robotis.com/docs/en/dxl/protocol1/
denis2nis 8:e74ef93ae660 304 *
denis2nis 13:ccda9a56fef1 305 * Structure of instruction Packet
denis2nis 13:ccda9a56fef1 306 *
denis2nis 8:e74ef93ae660 307 * <PRE>
denis2nis 8:e74ef93ae660 308 * |Header1|Header2|Packet ID|Length|Instruction|Param1...ParamN|Checksum|
denis2nis 8:e74ef93ae660 309 * |-------|-------|---------|------|-----------|---------------|--------|
denis2nis 8:e74ef93ae660 310 * | 0xFF | 0xFF |Packet ID|Length|Instruction|Param1...ParamN| CHKSUM |
denis2nis 8:e74ef93ae660 311 * | cmd[0]| cmd[1]| cmd[2] |cmd[3]| cmd[4] |cmd[5]... | |
denis2nis 8:e74ef93ae660 312 * \\__ ___/ \\_ _/ \\__ __/
denis2nis 8:e74ef93ae660 313 * \/ \/ \/
denis2nis 8:e74ef93ae660 314 * mot_id address data
denis2nis 8:e74ef93ae660 315 * (len = N-1)
denis2nis 8:e74ef93ae660 316 * </PRE>
denis2nis 8:e74ef93ae660 317 */
denis2nis 8:e74ef93ae660 318 void rw(unsigned char mot_id, char address, char len, char *data);
denis2nis 8:e74ef93ae660 319
denis2nis 13:ccda9a56fef1 320 /**
denis2nis 13:ccda9a56fef1 321 * Display status packet in hexadecimal format
denis2nis 8:e74ef93ae660 322 *
denis2nis 8:e74ef93ae660 323 * @warning
denis2nis 13:ccda9a56fef1 324 * Use console without any previous declaration. Assume console is
denis2nis 13:ccda9a56fef1 325 * setup by calling program
denis2nis 8:e74ef93ae660 326 */
denis2nis 8:e74ef93ae660 327 void PrintSerial();
denis2nis 8:e74ef93ae660 328
denis2nis 8:e74ef93ae660 329 /**
denis2nis 8:e74ef93ae660 330 * Get information from de Error byte of the last status packet
denis2nis 8:e74ef93ae660 331 * received from a servomotor
denis2nis 8:e74ef93ae660 332 *
denis2nis 8:e74ef93ae660 333 * @return
denis2nis 8:e74ef93ae660 334 * One of enum MX12::Status value to describe errors. 'Ok' is
denis2nis 8:e74ef93ae660 335 * returns if no error occurred during the last operation of
denis2nis 8:e74ef93ae660 336 * servomotor.
denis2nis 8:e74ef93ae660 337 *
denis2nis 8:e74ef93ae660 338 * @warning
denis2nis 10:ca9afe156ee1 339 * Warning: if a combination of several errors is reported the function
denis2nis 14:5a0c7b30f8c0 340 * returns "Unknown".
denis2nis 9:b4a5187fdec6 341 *
denis2nis 16:8fcf79e02dd4 342 * @bug
denis2nis 10:ca9afe156ee1 343 * Bug: this method has a hazardous behavior because it provides
denis2nis 10:ca9afe156ee1 344 * a result by reading the content of the variable _current_frame
denis2nis 13:ccda9a56fef1 345 * which can be modified at any time by the private ISR _Rx_interrupt()
denis2nis 8:e74ef93ae660 346 */
denis2nis 8:e74ef93ae660 347 MX12::Status GetStatus(void);
denis2nis 5:0cf54586a4be 348
denis2nis 8:e74ef93ae660 349 /* function aivailaible in a previous version of the class
denis2nis 8:e74ef93ae660 350 */
denis2nis 8:e74ef93ae660 351 // MX12::Status GetStatus(void);
denis2nis 8:e74ef93ae660 352 // void ReadPosition(unsigned char mot_id);
denis2nis 8:e74ef93ae660 353 // float GetPosition(unsigned char mot_id);
denis2nis 8:e74ef93ae660 354
denis2nis 10:ca9afe156ee1 355 protected:
denis2nis 5:0cf54586a4be 356
denis2nis 13:ccda9a56fef1 357 /** Serial port where servomotor bus is connected
denis2nis 9:b4a5187fdec6 358 */
denis2nis 8:e74ef93ae660 359 UnbufferedSerial _mx12;
denis2nis 13:ccda9a56fef1 360
denis2nis 13:ccda9a56fef1 361
denis2nis 13:ccda9a56fef1 362 /** Servomotor bus state to managed medium acces control (MAC)
denis2nis 13:ccda9a56fef1 363 */
denis2nis 13:ccda9a56fef1 364 MX12::SerialState _bus_state;
denis2nis 8:e74ef93ae660 365
denis2nis 13:ccda9a56fef1 366 /** Structure filled by ISR (Interrupt Service Routine) ReadCallback()
denis2nis 13:ccda9a56fef1 367 * parser to store message received from servomotor byte after byte
denis2nis 8:e74ef93ae660 368 */
denis2nis 13:ccda9a56fef1 369 MX12::Status_packet _status_pck;
denis2nis 13:ccda9a56fef1 370
denis2nis 13:ccda9a56fef1 371 /** Structure update by ISR (Interrupt Service Routine) ReadCallback()
denis2nis 13:ccda9a56fef1 372 * parser to model its current state
denis2nis 13:ccda9a56fef1 373 */
denis2nis 13:ccda9a56fef1 374 MX12::ParserState _parser_state;
denis2nis 13:ccda9a56fef1 375
denis2nis 8:e74ef93ae660 376
denis2nis 8:e74ef93ae660 377 unsigned char _answer;
denis2nis 13:ccda9a56fef1 378
denis2nis 13:ccda9a56fef1 379 /**
denis2nis 13:ccda9a56fef1 380 * @brief
denis2nis 13:ccda9a56fef1 381 * Interupt service routine (ISR) to read and parse received message
denis2nis 13:ccda9a56fef1 382 *
denis2nis 13:ccda9a56fef1 383 * @detail
denis2nis 13:ccda9a56fef1 384 * This interupt routine is attached to reception event on servomoteur
denis2nis 13:ccda9a56fef1 385 * bus and is called for each character received on bus.
denis2nis 13:ccda9a56fef1 386 *
denis2nis 13:ccda9a56fef1 387 * As the characters are received, the progress of the parser
denis2nis 13:ccda9a56fef1 388 * is stored in _parser_state and the result of the analysis
denis2nis 13:ccda9a56fef1 389 * is stored in _status_pck.
denis2nis 8:e74ef93ae660 390 */
denis2nis 13:ccda9a56fef1 391 void _Rx_interrupt();
denis2nis 8:e74ef93ae660 392
denis2nis 0:7556356a8bcd 393 };
denis2nis 0:7556356a8bcd 394
denis2nis 8:e74ef93ae660 395 #endif /* MBED_MX12_H_ */