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@27:06850c65b9c8, 2021-11-26 (annotated)
- Committer:
- lorenzodunau
- Date:
- Fri Nov 26 08:55:31 2021 +0000
- Revision:
- 27:06850c65b9c8
- Parent:
- 26:4632a02a8ef1
- Child:
- 28:c7402e1014b4
Who changed what in which revision?
| User | Revision | Line number | New 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); |
| lorenzodunau | 27:06850c65b9c8 | 353 | // envoie la commande moteur de manière à avancer d'une vitesse Vavance en |
| lorenzodunau | 27:06850c65b9c8 | 354 | // mètres par seconde, d'une vitesse Vlat en mètre par seconde et d'une |
| lorenzodunau | 27:06850c65b9c8 | 355 | // vitesse de rotation Wz en rad/s |
| lorenzodunau | 27:06850c65b9c8 | 356 | |
| lorenzodunau | 27:06850c65b9c8 | 357 | void eteindre_moteurs(); |
| lorenzodunau | 27:06850c65b9c8 | 358 | |
| lorenzodunau | 27:06850c65b9c8 | 359 | |
| denis2nis | 8:e74ef93ae660 | 360 | |
| denis2nis | 10:ca9afe156ee1 | 361 | protected: |
| denis2nis | 5:0cf54586a4be | 362 | |
| denis2nis | 13:ccda9a56fef1 | 363 | /** Serial port where servomotor bus is connected |
| denis2nis | 9:b4a5187fdec6 | 364 | */ |
| denis2nis | 8:e74ef93ae660 | 365 | UnbufferedSerial _mx12; |
| denis2nis | 13:ccda9a56fef1 | 366 | |
| denis2nis | 13:ccda9a56fef1 | 367 | |
| denis2nis | 13:ccda9a56fef1 | 368 | /** Servomotor bus state to managed medium acces control (MAC) |
| denis2nis | 13:ccda9a56fef1 | 369 | */ |
| denis2nis | 13:ccda9a56fef1 | 370 | MX12::SerialState _bus_state; |
| denis2nis | 8:e74ef93ae660 | 371 | |
| denis2nis | 13:ccda9a56fef1 | 372 | /** Structure filled by ISR (Interrupt Service Routine) ReadCallback() |
| denis2nis | 13:ccda9a56fef1 | 373 | * parser to store message received from servomotor byte after byte |
| denis2nis | 8:e74ef93ae660 | 374 | */ |
| denis2nis | 13:ccda9a56fef1 | 375 | MX12::Status_packet _status_pck; |
| denis2nis | 13:ccda9a56fef1 | 376 | |
| denis2nis | 13:ccda9a56fef1 | 377 | /** Structure update by ISR (Interrupt Service Routine) ReadCallback() |
| denis2nis | 13:ccda9a56fef1 | 378 | * parser to model its current state |
| denis2nis | 13:ccda9a56fef1 | 379 | */ |
| denis2nis | 13:ccda9a56fef1 | 380 | MX12::ParserState _parser_state; |
| denis2nis | 13:ccda9a56fef1 | 381 | |
| denis2nis | 8:e74ef93ae660 | 382 | |
| denis2nis | 8:e74ef93ae660 | 383 | unsigned char _answer; |
| denis2nis | 13:ccda9a56fef1 | 384 | |
| denis2nis | 13:ccda9a56fef1 | 385 | /** |
| denis2nis | 13:ccda9a56fef1 | 386 | * @brief |
| denis2nis | 13:ccda9a56fef1 | 387 | * Interupt service routine (ISR) to read and parse received message |
| denis2nis | 13:ccda9a56fef1 | 388 | * |
| denis2nis | 13:ccda9a56fef1 | 389 | * @detail |
| denis2nis | 13:ccda9a56fef1 | 390 | * This interupt routine is attached to reception event on servomoteur |
| denis2nis | 13:ccda9a56fef1 | 391 | * bus and is called for each character received on bus. |
| denis2nis | 13:ccda9a56fef1 | 392 | * |
| denis2nis | 13:ccda9a56fef1 | 393 | * As the characters are received, the progress of the parser |
| denis2nis | 13:ccda9a56fef1 | 394 | * is stored in _parser_state and the result of the analysis |
| denis2nis | 13:ccda9a56fef1 | 395 | * is stored in _status_pck. |
| denis2nis | 8:e74ef93ae660 | 396 | */ |
| denis2nis | 13:ccda9a56fef1 | 397 | void _Rx_interrupt(); |
| denis2nis | 8:e74ef93ae660 | 398 | |
| denis2nis | 0:7556356a8bcd | 399 | }; |
| denis2nis | 0:7556356a8bcd | 400 | |
| denis2nis | 8:e74ef93ae660 | 401 | #endif /* MBED_MX12_H_ */ |