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@4:277e5a4cba2e, 2021-11-04 (annotated)

Committer:
denis2nis
Date:
Thu Nov 04 07:32:20 2021 +0000
Revision:
4:277e5a4cba2e
Parent:
3:add8b050eb86
Child:
5:0cf54586a4be
Arrange and comment (cont.)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
denis2nis 0:7556356a8bcd 1 #ifndef MBED_MX12_H
denis2nis 0:7556356a8bcd 2 #define MBED_MX12_H
denis2nis 0:7556356a8bcd 3
denis2nis 1:a9ba9cf928fe 4 /**
denis2nis 1:a9ba9cf928fe 5 * @file MX12.h
denis2nis 1:a9ba9cf928fe 6 * @brief this header file will contain all required
denis2nis 1:a9ba9cf928fe 7 * definitions and basic utilities functions.
denis2nis 2:02f3323a107d 8 * @details comming soon
denis2nis 1:a9ba9cf928fe 9 *
denis2nis 1:a9ba9cf928fe 10 * @author Titouan Soulard
denis2nis 1:a9ba9cf928fe 11 * @author Bruno Denis (for comments)
denis2nis 1:a9ba9cf928fe 12 *
denis2nis 1:a9ba9cf928fe 13 */
denis2nis 0:7556356a8bcd 14
denis2nis 0:7556356a8bcd 15 #include "mbed.h"
denis2nis 0:7556356a8bcd 16
denis2nis 0:7556356a8bcd 17 #define MX12_ANSWER_MAX_SIZE 32
denis2nis 0:7556356a8bcd 18 #define MX12_MOTOR_COUNT 16
denis2nis 0:7556356a8bcd 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 2:02f3323a107d 32
denis2nis 2:02f3323a107d 33
denis2nis 2:02f3323a107d 34 /**
denis2nis 2:02f3323a107d 35 * @brief Class to communicate with Dynamixel MX12 servomotors.
denis2nis 1:a9ba9cf928fe 36 *
denis2nis 2:02f3323a107d 37 * @details
denis2nis 2:02f3323a107d 38 * The servomotors are daisy chained to a serial link of the target
denis2nis 2:02f3323a107d 39 * microcontroller. The class ensures the initialization of serial link
denis2nis 2:02f3323a107d 40 * and the management of communications.
denis2nis 2:02f3323a107d 41 * Transmission of messages to the servomotors is blocking while
denis2nis 2:02f3323a107d 42 * reception is non-blocking thanks to the use of an interrupt routine.
denis2nis 1:a9ba9cf928fe 43 *
denis2nis 1:a9ba9cf928fe 44 */
denis2nis 0:7556356a8bcd 45 class MX12
denis2nis 0:7556356a8bcd 46 {
denis2nis 0:7556356a8bcd 47 public:
denis2nis 0:7556356a8bcd 48
denis2nis 3:add8b050eb86 49 /** Error status occurred during the operation of servomotor.
denis2nis 1:a9ba9cf928fe 50 *
denis2nis 3:add8b050eb86 51 * BDenis remarj: enum type is not suitable for the errors status
denis2nis 3:add8b050eb86 52 * because several errors can be repported simultaneously
denis2nis 1:a9ba9cf928fe 53 */
denis2nis 0:7556356a8bcd 54 enum Status {
denis2nis 3:add8b050eb86 55 InstructionError, ///< In case of sending an undefined instruction or delivering the action instruction without the Reg Write instruction
denis2nis 3:add8b050eb86 56 OverloadError, ///< When the current load cannot be controlled by the set Torque
denis2nis 3:add8b050eb86 57 ChecksumError, ///< When the Checksum of the transmitted Instruction Packet is incorrect
denis2nis 3:add8b050eb86 58 RangeError, ///< When an instruction is out of the range for use
denis2nis 3:add8b050eb86 59 OverheatingError, ///< When internal temperature of servomotor is out of the range of operating temperature set in the Control table
denis2nis 3:add8b050eb86 60 AngleLimitError, ///< When Goal Position is written out of the range from CW Angle Limit to CCW Angle Limit
denis2nis 3:add8b050eb86 61 InputVoltageError, ///< When the applied voltage is out of the range of operating voltage set in the Control table
denis2nis 3:add8b050eb86 62 Unknown, ///< ???
denis2nis 3:add8b050eb86 63 Ok ///< no error
denis2nis 0:7556356a8bcd 64 };
denis2nis 0:7556356a8bcd 65
denis2nis 2:02f3323a107d 66 /** State enum, possible state of communication automaton
denis2nis 1:a9ba9cf928fe 67 *
denis2nis 1:a9ba9cf928fe 68 */
denis2nis 1:a9ba9cf928fe 69 enum State {
denis2nis 3:add8b050eb86 70 ReadingPosition, ///< ReadingPosition
denis2nis 3:add8b050eb86 71 Writing, ///< Writing
denis2nis 3:add8b050eb86 72 Available, ///< Available
denis2nis 0:7556356a8bcd 73 };
denis2nis 0:7556356a8bcd 74
denis2nis 0:7556356a8bcd 75 /** Create MX12 instance
denis2nis 0:7556356a8bcd 76 *
denis2nis 0:7556356a8bcd 77 * @param tx board pin used for transmission in UART daisy chain link
denis2nis 0:7556356a8bcd 78 * to servomotors
denis2nis 0:7556356a8bcd 79 * @param rx board pin used for reception in UART daisy chain link
denis2nis 0:7556356a8bcd 80 * to servomotors
denis2nis 0:7556356a8bcd 81 * @param baud modulation rate of UART signal, unit: Baud
denis2nis 0:7556356a8bcd 82 * to servomotors
denis2nis 0:7556356a8bcd 83 */
denis2nis 0:7556356a8bcd 84 MX12(PinName tx, PinName rx, int baud=115200);
denis2nis 0:7556356a8bcd 85
denis2nis 0:7556356a8bcd 86 /** Send desired speed to a specifc servomotor
denis2nis 0:7556356a8bcd 87 *
denis2nis 0:7556356a8bcd 88 * @param mot_id a unique value in the daisy chain network to identify
denis2nis 0:7556356a8bcd 89 * each servomotor
denis2nis 0:7556356a8bcd 90 * @param speed a float between -1 and 1 that represents the percentage
denis2nis 0:7556356a8bcd 91 * of maximum requested speed
denis2nis 0:7556356a8bcd 92 */
denis2nis 0:7556356a8bcd 93 void SetSpeed(unsigned char mot_id, float speed);
denis2nis 0:7556356a8bcd 94
denis2nis 0:7556356a8bcd 95 char IsAvailable(void);
denis2nis 0:7556356a8bcd 96
denis2nis 0:7556356a8bcd 97 MX12::Status GetStatus(void);
denis2nis 0:7556356a8bcd 98
denis2nis 1:a9ba9cf928fe 99
denis2nis 0:7556356a8bcd 100 void ReadPosition(unsigned char mot_id);
denis2nis 0:7556356a8bcd 101
denis2nis 0:7556356a8bcd 102 float GetPosition(unsigned char mot_id);
denis2nis 0:7556356a8bcd 103
denis2nis 0:7556356a8bcd 104 void PrintAnswer();
denis2nis 0:7556356a8bcd 105
denis2nis 2:02f3323a107d 106 /**
denis2nis 2:02f3323a107d 107 * Build and send an instruction packet to a servomotor according
denis2nis 2:02f3323a107d 108 * DYNAMIXEL Protocol 1.0 (online protocol documentation
denis2nis 2:02f3323a107d 109 * https://emanual.robotis.com/docs/en/dxl/protocol1/)
denis2nis 2:02f3323a107d 110 *
denis2nis 2:02f3323a107d 111 * @param[in] mot_id indicates the ID of the device (servomotor) that
denis2nis 2:02f3323a107d 112 * should receive the Instruction Packet and process it
denis2nis 2:02f3323a107d 113 * @param[in] address data address in the "Control Table of RAM Area"
denis2nis 2:02f3323a107d 114 * of a servomotor MX12 (https://emanual.robotis.com/docs/en/dxl/mx/mx-12w/#control-table-of-ram-area).
denis2nis 2:02f3323a107d 115 * @param[in] len if it is a write instruction, size in bytes
denis2nis 2:02f3323a107d 116 * of the data to write in the "Control Table of RAM Area"
denis2nis 3:add8b050eb86 117 * @param[in, out] data array of char containing parameter of Danamixel
denis2nis 2:02f3323a107d 118 * protocol that are the instruction’s auxiliary data field
denis2nis 2:02f3323a107d 119 *
denis2nis 3:add8b050eb86 120 * @see https://emanual.robotis.com/docs/en/dxl/protocol1/
denis2nis 3:add8b050eb86 121 *
denis2nis 3:add8b050eb86 122 * Packet sent
denis2nis 4:277e5a4cba2e 123 * <PRE>
denis2nis 3:add8b050eb86 124 * |Header1|Header2|Packet ID|Length|Instruction|Param1...ParamN|Checksum |
denis2nis 3:add8b050eb86 125 * |-------|-------|---------|------|-----------|---------------|---------|
denis2nis 3:add8b050eb86 126 * | 0xFF | 0xFF |Packet ID|Length|Instruction|Param1...ParamN| CHKSUM |
denis2nis 3:add8b050eb86 127 * | cmd[0]| cmd[1]| cmd[2] |cmd[3]| cmd[4] |cmd[5]... |(2 bytes)|
denis2nis 3:add8b050eb86 128 * \_ _/ \__ __/
denis2nis 3:add8b050eb86 129 * \/ \/
denis2nis 3:add8b050eb86 130 * address data
denis2nis 3:add8b050eb86 131 * (len = N-1)
denis2nis 4:277e5a4cba2e 132 * </PRE>
denis2nis 2:02f3323a107d 133 */
denis2nis 2:02f3323a107d 134 void rw(unsigned char mot_id, char address, char len, char *data);
denis2nis 0:7556356a8bcd 135
denis2nis 1:a9ba9cf928fe 136 /** Interupt Routine to read in data from UART daisy chain link
denis2nis 1:a9ba9cf928fe 137 * (serial port)
denis2nis 1:a9ba9cf928fe 138 *
denis2nis 1:a9ba9cf928fe 139 */
denis2nis 0:7556356a8bcd 140 void _ReadCallback();
denis2nis 0:7556356a8bcd 141
denis2nis 0:7556356a8bcd 142 char _chksm;
denis2nis 0:7556356a8bcd 143
denis2nis 0:7556356a8bcd 144 private:
denis2nis 0:7556356a8bcd 145
denis2nis 0:7556356a8bcd 146 UnbufferedSerial _mx12;
denis2nis 0:7556356a8bcd 147 MX12::Status _status;
denis2nis 0:7556356a8bcd 148 char _res[MX12_ANSWER_MAX_SIZE];
denis2nis 0:7556356a8bcd 149 char _res_count;
denis2nis 0:7556356a8bcd 150 char _len;
denis2nis 0:7556356a8bcd 151 MX12::State _state;
denis2nis 0:7556356a8bcd 152 float _angle[MX12_MOTOR_COUNT];
denis2nis 0:7556356a8bcd 153 int _baud;
denis2nis 0:7556356a8bcd 154 };
denis2nis 0:7556356a8bcd 155
denis2nis 0:7556356a8bcd 156 #endif /* MBED_MX12_H */