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
Diff: MX12.h
- Revision:
- 8:e74ef93ae660
- Parent:
- 7:68983a9b0531
- Child:
- 9:b4a5187fdec6
--- a/MX12.h Thu Nov 04 15:11:58 2021 +0000
+++ b/MX12.h Sat Nov 06 08:29:28 2021 +0000
@@ -1,13 +1,12 @@
/**
-* @file MX12.h
-* @brief this header file will contain all required
-* definitions and basic utilities functions.
-* @details comming soon
-*
-*
-*/
-#ifndef MBED_MX12_H
-#define MBED_MX12_H
+ * @file MX12.h
+ * @brief this header file will contain all required definitions and
+ * basic utilities functions to manage au bus of servomotor
+ * Dynaminel MX12
+ *
+ */
+#ifndef MBED_MX12_H_
+#define MBED_MX12_H_
#include "mbed.h"
@@ -64,15 +63,17 @@
#define CONTROL_TABLE_GOAL_ACCELERATION 73
/**
- * @brief Class to communicate with Dynamixel MX12 servomotors.
+ * @brief Class to communicate with Dynamixel MX12 servomotors
*
* @details
+ *
* The servomotors are daisy chained to a serial link of the target
* microcontroller. The class ensures the initialization of serial link
- * and the management of communications.
+ * and the management of communications mainly to control rotational
+ * speed of servomotors.
+ *
* Transmission of messages to the servomotors is blocking while
* reception is non-blocking thanks to the use of an interrupt routine.
- *
*
* @author Titouan Soulard (creator and maintainer until April 2021)
* @author Bruno Denis (Doxygen documentation and code rearrangement)
@@ -82,184 +83,222 @@
* @see Dynamixel protocol v1.0 manual
* @see https://emanual.robotis.com/docs/en/dxl/protocol1/
*
- * @warning
- * Error field of status packet if decoded by GetStatus(() function which
- * returns an enumation type "Status". As several errors can be reported
- * simultaneously the type enum is not suitable (B. Denis 11/2021)
+ * @warning
+ *
+ * Error field of status packet if decoded by GetStatus(() function which
+ * returns an enumation type "Status". As several errors can be reported
+ * simultaneously the type enum is not suitable (B. Denis 11/2021)
+ *
+ * _frame_pointer variable is used by _ReadCallback() ISR to store the
+ * current size for message (status packet) received from servomotor.
+ * This variable is NOT initialised and NEVER reset to zero at each new
+ * message.
+ *
*/
class MX12
{
public:
- /** Error status occurred during the operation of servomotor.
- *
- * @warning
- * Enumaration type is not suitable for all error status because
- * it can denote only one error at a time (B. Denis 11/2021)
- */
- enum Status {
- InstructionError, /**< In case of sending an undefined instruction
- or delivering the action instruction
- without the Reg Write instruction */
- OverloadError, /**< When the current load cannot be controlled
- by the set Torque */
- ChecksumError, /**< When the Checksum of the transmitted
- Instruction Packet is incorrect */
- RangeError, /**< When an instruction is out of the range
- for use */
- OverheatingError, /**< When internal temperature of servomotor
- is out of the range of operating
- temperature set in the Control table */
- AngleLimitError, /**< When Goal Position is written out of the
- range from CW Angle Limit to CCW Angle
- Limit */
- InputVoltageError, /**< When the applied voltage is out of the
- range of operating voltage set in the
- Control table */
- Unknown, /**< Combination of several errors (Caution:
- limit of enum approach to code errors) */
- Ok ///< no error
- };
-
- /** State of packet parser to determine the meaning of reading byte
- * into the received packet (satuts packet) according the Dynamixel
- * protocol v1.0
- */
- enum ParsingState {
- Header, ///< reading the two heading bytes
- Id, ///< reading the servomotor ID byte
- Length, ///< reading byte length of the instruction
- Data, ///< reading parameters
- Checksum, ///< reading one bytes checksum
- };
+ /** Error status occurred during the operation of servomotor.
+ *
+ * @warning
+ * Enumaration type is not suitable for all error status because
+ * it can denote only one error at a time (B. Denis 11/2021)
+ */
+ enum Status {
+ InstructionError, /**< In case of sending an undefined instruction
+ or delivering the action instruction
+ without the Reg Write instruction */
+ OverloadError, /**< When the current load cannot be controlled
+ by the set Torque */
+ ChecksumError, /**< When the Checksum of the transmitted
+ Instruction Packet is incorrect */
+ RangeError, /**< When an instruction is out of the range
+ for use */
+ OverheatingError, /**< When internal temperature of servomotor
+ is out of the range of operating
+ temperature set in the Control table */
+ AngleLimitError, /**< When Goal Position is written out of the
+ range from CW Angle Limit to CCW Angle
+ Limit */
+ InputVoltageError, /**< When the applied voltage is out of the
+ range of operating voltage set in the
+ Control table */
+ Unknown, /**< Combination of several errors (Caution:
+ limit of enum approach to code errors) */
+ Ok ///< no error
+ };
+
+ /** State of packet parser to determine the meaning of reading byte
+ * into the received packet (status packet) according the Dynamixel
+ * protocol v1.0
+ */
+ enum ParsingState {
+ Header, ///< reading the two heading bytes
+ Id, ///< reading the servomotor ID byte
+ Length, ///< reading byte length of the instruction
+ Data, ///< reading parameters
+ Checksum ///< reading one bytes checksum
+ };
- /** Enumeration of states of the acces methode of the master-slave
- * protocol of the communication between the robot controller (master)
- * and the servomotors (slaves).
- */
- enum SerialState {
- Writing, /**< Robot controller send an "instruction packet"
- (request) to a servomotor */
- Reading, /**< Robot controller receive a "status packet" from a
- requested servomotor */
- Idle, ///< Robot controller ready for a new cycle request-answer
- };
-
- /** type
- */
- struct Frame {
- unsigned char motorId;
- unsigned char length;
- unsigned char data[MX12_DATA_MAX_SIZE];
- unsigned char valid;
- };
+ /** Enumeration of states of the acces methode of the master-slave
+ * protocol of the communication between the robot controller (master)
+ * and the servomotors (slaves).
+ */
+ enum SerialState {
+ Writing, /**< Robot controller send an "instruction packet"
+ (request) to a servomotor */
+ Reading, /**< Robot controller receive a "status packet" from a
+ requested servomotor */
+ Idle ///< Robot controller ready for a new cycle request-answer
+ };
+
+ /** Structure of store status packet (also known as return packet)
+ * according tne Dynamixel protocol v1.0, which are messages sent
+ * by servomotors to contriller in response of an instruction packet
+ */
+ struct Frame {
+ unsigned char motorId; /**< Identifier of the servomotor involved
+ in this exchange */
+ unsigned char length; /**< */
+ unsigned char data[MX12_DATA_MAX_SIZE]; /**< */
+ unsigned char valid; /**< */
+ };
+
+ /**
+ */
+ struct StateContext {
+ unsigned char headingCount;
+ unsigned char dataCount;
+ unsigned char checksum;
+ };
- /**
- */
- struct StateContext {
- unsigned char headingCount;
- unsigned char dataCount;
- unsigned char checksum;
- };
-
- /** Create MX12 instance
- *
- * @param tx board pin used for transmission in UART daisy chain link
- * to servomotors
- * @param rx board pin used for reception in UART daisy chain link
- * to servomotors
- * @param baud modulation rate of UART signal, unit: Baud
- * to servomotors
- */
- MX12(PinName tx, PinName rx, int baud=115200);
-
- /** Send desired speed to a specifc servomotor
- *
- * @param mot_id a unique value in the daisy chain network to identify
- * each servomotor
- * @param speed a float between -1 and 1 that represents the percentage
- * of maximum requested speed
- */
- void SetSpeed(unsigned char mot_id, float speed);
-
- char IsAvailable(void);
-
- /**
- * Build and send an instruction packet to a servomotor according
- * DYNAMIXEL Protocol 1.0 (online protocol documentation
- * https://emanual.robotis.com/docs/en/dxl/protocol1/)
- *
- * @param[in] mot_id indicates the ID of the device (servomotor) that
- * should receive the Instruction Packet and process it
- * @param[in] address data address in the "Control Table of RAM Area"
- * of a servomotor MX12
- * (https://emanual.robotis.com/docs/en/dxl/mx/mx-12w/#control-table-of-ram-area).
- * @param[in] len if it is a write instruction, size in bytes
- * of the data to write in the "Control Table of RAM Area"
- * @param[in] data array of char containing parameter of Danamixel
- * protocol that are the instruction’s auxiliary data field
- *
- * @see https://emanual.robotis.com/docs/en/dxl/protocol1/
- *
- * Packet sent
- * <PRE>
- * |Header1|Header2|Packet ID|Length|Instruction|Param1...ParamN|Checksum|
- * |-------|-------|---------|------|-----------|---------------|--------|
- * | 0xFF | 0xFF |Packet ID|Length|Instruction|Param1...ParamN| CHKSUM |
- * | cmd[0]| cmd[1]| cmd[2] |cmd[3]| cmd[4] |cmd[5]... | |
- * \\__ ___/ \\_ _/ \\__ __/
- * \/ \/ \/
- * mot_id address data
- * (len = N-1)
- * </PRE>
- */
- void rw(unsigned char mot_id, char address, char len, char *data);
-
- /**
- *
- * @warning
- *
- */
- void PrintSerial();
-
- /**
- * Get information from de Error byte of the last status packet
- * received from a servomotor
- *
- * @return
- * One of enum MX12::Status value to describe errors. 'Ok' is
- * returns if no error occurred during the last operation of
- * servomotor.
- *
- * @warning
- * If a combination of several errors is reported the function
- * returns 'Unknown' (B. Denis 11/2021)
- */
- MX12::Status GetStatus(void);
+ /** Create MX12 instance
+ *
+ * @param tx board pin used for transmission in UART daisy chain link
+ * to servomotors
+ * @param rx board pin used for reception in UART daisy chain link
+ * to servomotors
+ * @param baud modulation rate of UART signal, unit: Baud
+ * to servomotors
+ */
+ MX12(PinName tx, PinName rx, int baud=115200);
+
+ /** Send desired speed to a specific servomotor
+ *
+ * @param mot_id a unique value in the daisy chain network to identify
+ * each servomotor
+ * @param speed a float between -1 and 1 that represents the percentage
+ * of maximum requested speed
+ */
+ void SetSpeed(unsigned char mot_id, float speed);
+
+ char IsAvailable(void);
+
+ /**
+ * Build and send an instruction packet to a servomotor according
+ * DYNAMIXEL Protocol 1.0 (online protocol documentation
+ * https://emanual.robotis.com/docs/en/dxl/protocol1/)
+ *
+ * @param[in] mot_id indicates the ID of the device (servomotor) that
+ * should receive the Instruction Packet and process it
+ * @param[in] address data address in the "Control Table of RAM Area"
+ * of a servomotor MX12
+ * (https://emanual.robotis.com/docs/en/dxl/mx/mx-12w/#control-table-of-ram-area).
+ * @param[in] len if it is a write instruction, size in bytes
+ * of the data to write in the "Control Table of RAM Area"
+ * @param[in] data array of char containing parameter of Danamixel
+ * protocol that are the instruction’s auxiliary data field
+ *
+ * @see https://emanual.robotis.com/docs/en/dxl/protocol1/
+ *
+ * Packet sent
+ * <PRE>
+ * |Header1|Header2|Packet ID|Length|Instruction|Param1...ParamN|Checksum|
+ * |-------|-------|---------|------|-----------|---------------|--------|
+ * | 0xFF | 0xFF |Packet ID|Length|Instruction|Param1...ParamN| CHKSUM |
+ * | cmd[0]| cmd[1]| cmd[2] |cmd[3]| cmd[4] |cmd[5]... | |
+ * \\__ ___/ \\_ _/ \\__ __/
+ * \/ \/ \/
+ * mot_id address data
+ * (len = N-1)
+ * </PRE>
+ */
+ void rw(unsigned char mot_id, char address, char len, char *data);
+
+ /**
+ *
+ * @warning
+ *
+ */
+ void PrintSerial();
+
+ /**
+ * Get information from de Error byte of the last status packet
+ * received from a servomotor
+ *
+ * @return
+ * One of enum MX12::Status value to describe errors. 'Ok' is
+ * returns if no error occurred during the last operation of
+ * servomotor.
+ *
+ * @warning
+ * If a combination of several errors is reported the function
+ * returns 'Unknown' (B. Denis 11/2021)
+ */
+ MX12::Status GetStatus(void);
- /* function aivailaible in a previous version of the class
- */
- // MX12::Status GetStatus(void);
- // void ReadPosition(unsigned char mot_id);
- // float GetPosition(unsigned char mot_id);
-
+ /* function aivailaible in a previous version of the class
+ */
+ // MX12::Status GetStatus(void);
+ // void ReadPosition(unsigned char mot_id);
+ // float GetPosition(unsigned char mot_id);
+ int get_counter();
+
+
private:
- UnbufferedSerial _mx12;
- MX12::ParsingState _pstate;
- MX12::SerialState _sstate;
- MX12::StateContext _scontext;
- MX12::Frame _current_frame;
-
- unsigned char _answer;
- unsigned char _stored_frame[MX12_DATA_MAX_SIZE];
- unsigned char _frame_pointer;
-
- /** Interupt Routine to read in data from UART daisy chain link
- * (serial port)
- *
- */
- void _ReadCallback();
+ UnbufferedSerial _mx12;
+ MX12::ParsingState _pstate;
+ MX12::SerialState _sstate;
+
+ /** Structure used only by ISR (Interrupt Service Routine) ReadCallback()
+ *
+ */
+ MX12::StateContext _scontext;
+ MX12::Frame _current_frame;
+
+ unsigned char _answer;
+
+ /** Character array to store status message received from servomotors.
+ * _ReadCallback() ISR fill this array character by character and
+ * update the size of the message currently received in the variable
+ * _frame_pointer.
+ */
+ unsigned char _stored_frame[MX12_DATA_MAX_SIZE];
+
+ /** Size of the message currently received from servomotor. This variable
+ * is update at each call of _ReadCallback() ISR. The content of the
+ * message is store into _stored_frame array.
+ *
+ * @warning
+ */
+ unsigned char _frame_pointer;
+
+ volatile int _BDcount;
+
+ /** Interupt service routine (ISR) to read and parse the response message
+ * comming from servomotor on UART (serial port). This routine is called
+ * when each character is received on the servomotor bus.
+ *
+ * As the characters are received, the progress of the parser
+ * is stored in the two state variables _pstate and _scontext,
+ * and the result of the analysis is stored in _current_frame.
+ */
+ void _ReadCallback();
+
+ void _Rx_BD_interrupt();
+
};
-#endif /* MBED_MX12_H */
\ No newline at end of file
+#endif /* MBED_MX12_H_ */
\ No newline at end of file