PRISMA Lab
mbedos senza corrente
Diff: MX.h
- Revision:
- 17:6f85605f793d
- Child:
- 18:862a4afc1fbe
- Child:
- 19:16d5f8a3311f
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/MX.h Fri Jul 26 12:40:22 2019 +0000
@@ -0,0 +1,228 @@
+/* mbed MX-64 Servo Library
+ *
+ * Copyright (c) 2010, cstyles (http://mbed.org)
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#ifndef MBED_MX_H
+#define MBED_MX_H
+
+#include "mbed.h"
+#include "UARTSerial_half.h"
+
+//#define SENDPACKET_DEBUG 1
+//#define SENDPACKET_DEBUG_INSTRUCTION_PKT 1
+//#define SENDPACKET_DEBUG_STATUS_PKT 1
+//#define READ_DEBUG 1
+//#define READ_DEBUG_INSTRUCTION_PKT 1
+//#define READ_DEBUG_STATUS_PKT 1
+//#define GETPOSITION_DEBUG 1
+
+//#define TORQUE_ENABLE_DEBUGDEBUG 1
+//#define REBOOT_ENABLE_DEBUG
+//#define SETGOAL_DEBUG 1
+//#define SETGOAL_SPEED_DEBUG 1
+//#define OPERATING_MODE_DEBUG 1
+//#define SETBAUD_DEBUG 1
+
+//#define SYNC_TORQUE_ENABLE_DEBUG 1
+//#define SYNC_SETGOAL_DEBUG 1
+//#define SYNC_GETPOSITION_DEBUG 1
+//#define SYNC_SENDPACKET_DEBUG 1
+//#define SYNC_SENDPACKET_DEBUG_PACKETONLY 1
+//#define SYNC_READPACKET_DEBUG_PACKETONLY 1
+//#define SYNC_SET_BAUD_DEBUG 1
+//#define SYNC_GOAL_CURRENT_DEBUG 1
+
+
+ // Protocol 2.0
+// N.B. all Register values are in HEX, on the on-line manual all are in DEC
+#define MX_REG_ID 0x7
+#define MX_REG_BAUD 0x8
+#define MX_REG_CW_LIMIT 0x30
+#define MX_REG_CCW_LIMIT 0x34
+#define MX_REG_GOAL_POSITION 0x74
+#define MX_REG_GOAL_VELOCITY 0x68
+#define MX_REG_PRESENT_INPUT_VOLT 0x90
+#define MX_REG_PRESENT_TEMP 0x92
+#define MX_REG_MOVING 0x7A
+#define MX_REG_PRESENT_POSITION 0x84
+#define MX_REG_TORQUE_ENABLE 0x40
+#define MX_REG_OPERATING_MODE 0xB
+#define MX_REG_PRESENT_CURRENT 0x7E
+#define MX_REG_GOAL_CURRENT 0x66
+#define MX_REG_CURRENT_LIMIT 0x26
+#define MX_OFF 0
+#define MX_ON 1
+
+class MX{
+
+
+public:
+
+ //standard constructor
+ MX();
+ //constructor
+ MX(int*broadcastID, int Nmotor, int MotorBaud, UARTSerial_half* pCommLayer);
+ //constructor 2
+ MX(int ID, int MotorBaud, UARTSerial_half* pCommLayer);
+
+ /** Set the mode of the servo
+ * @param mode
+ *
+ *
+ */
+ int OperatingMode (int mode);
+
+ /** Set baud rate of all attached servos
+ * @param mode
+ * 0x00 = 9,600 bps
+ * 0x01 = 57,600 bps (DEFAULT)
+ * 0x02 = 115,200 bps
+ * 0x03 = 1 Mbps
+ * 0x04 = 2 Mbps
+ * 0x05 = 3 Mbps
+ * 0x06 = 4 Mbps
+ * 0x06 = 4,5 Mbps
+ */
+ int motorSetBaud(int MotorBaud);
+
+ /** Set Torque enable
+ * enableVal = 1 Torque On
+ * enableVal = 0 Torque Off
+ **/
+ int TorqueEnable(bool enable);
+
+ /** Set goal angle in integer degrees, in positional mode
+ *
+ * @param degrees 0-300
+ * @param flags, defaults to 0
+ * flags[0] = blocking, return when goal position reached
+ * flags[1] = register, activate with a broadcast trigger
+ *
+ */
+ int SetGoal(float degrees, int flags = 0);
+ /** Set goal angle in integer degrees, in positional mode
+ *
+ * @param degrees 0-360
+ * @param speed 0-100
+ * @param flags, defaults to 0
+ * flags[0] = blocking, return when goal position reached
+ * flags[1] = register, activate with a broadcast trigger
+ *
+ */
+ int SetGoalSpeed(int speed, int flags = 0);
+
+
+ /** Set the speed of the servo in continuous rotation mode
+ *
+ * @param speed, -1.0 to 1.0
+ * -1.0 = full speed counter clock wise
+ * 1.0 = full speed clock wise
+ */
+ int SetCRSpeed(float speed);
+
+
+ /** Set the clockwise limit of the servo
+ *
+ * @param degrees, 0-360
+ */
+ int SetCWLimit(int degrees);
+
+ /** Set the counter-clockwise limit of the servo
+ *
+ * @param degrees, 0-360
+ */
+ int SetCCWLimit(int degrees);
+
+ // Change the ID
+
+ /** Change the ID of a servo
+ *
+ * @param CurentID 1-255
+ * @param NewID 1-255
+ *
+ * If a servo ID is not know, the broadcast address of 0 can be used for CurrentID.
+ * In this situation, only one servo should be connected to the bus
+ */
+ int SetID(int CurrentID, int NewID);
+
+
+ /** Poll to see if the servo is moving
+ *
+ * @returns true is the servo is moving
+ */
+ bool isMoving(void);
+
+ /** Send the broadcast "trigger" command, to activate any outstanding registered commands
+ */
+ void trigger(void);
+
+ /** Read the current angle of the servo
+ *
+ * @returns float in the range 0.0-300.0
+ */
+ float GetPosition();
+
+ /** Read the temperature of the servo
+ *
+ * @returns float temperature
+ */
+ float GetTemp(void);
+
+ /** Read the supply voltage of the servo
+ *
+ * @returns float voltage
+ */
+ float GetVolts(void);
+ /** ENABLE
+ *
+ * @returns float voltage
+ */
+
+ char* readPacket( int start, int length, int flag=0);
+ int sendPacket(int start, int length, char* data, int flag=0);
+ void Reboot();
+
+ char* SyncReadPacket(int start, int bytes);
+ float* SyncGetPosition(void);
+ int SyncTorqueEnable(bool enableVal[], int Nmotor =-1);
+ int SyncSetGoal(float degrees[]);
+ int SyncSetGoal(float degrees, int ID);
+ int SyncSendPacket(int start, int bytes, char* data, int ID =-1);
+ int SyncGoalCurrent(float mAmpere, int ID);
+ int SyncGoalCurrent(float mAmpere[]);
+ int SyncCurrentLimit(float mAmpere, int ID);
+ int SyncCurrentLimit(float mAmpere[]);
+ int SyncOperatingMode(int mode[], int ID);
+ int SyncSetBaud(int MotorBaud[], int ID);
+
+private :
+
+ UARTSerial_half* _pCommLayer;
+ int _ID;
+ int _MotorBaud;
+ int* _broadcastID;
+ int _Nmotor;
+
+ unsigned short update_crc(unsigned short crc_accum, char *data_blk_ptr, unsigned short data_blk_size);
+};
+
+#endif