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@26:4632a02a8ef1, 2021-11-26 (annotated)

Committer:
lorenzodunau
Date:
Fri Nov 26 07:16:53 2021 +0000
Revision:
26:4632a02a8ef1
Parent:
25:bc51575e3304
Child:
27:06850c65b9c8
derniere version 26/11/21

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