PRISMA Lab / MX64_senzaCorrente

mbedos senza corrente

MX.h@22:5d3f37356915, 2019-11-05 (annotated)

Committer:
marcodesilva
Date:
Tue Nov 05 14:26:34 2019 +0000
Revision:
22:5d3f37356915
Parent:
21:fe5dd48bebc6 
con Corrente e Settings Profili di Acc e Vel

Who changed what in which revision?

UserRevisionLine numberNew contents of line
marcodesilva 17:6f85605f793d 1 /* mbed MX-64 Servo Library
marcodesilva 17:6f85605f793d 2 *
marcodesilva 17:6f85605f793d 3 * Copyright (c) 2010, cstyles (http://mbed.org)
marcodesilva 17:6f85605f793d 4 *
marcodesilva 17:6f85605f793d 5 * Permission is hereby granted, free of charge, to any person obtaining a copy
marcodesilva 17:6f85605f793d 6 * of this software and associated documentation files (the "Software"), to deal
marcodesilva 17:6f85605f793d 7 * in the Software without restriction, including without limitation the rights
marcodesilva 17:6f85605f793d 8 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
marcodesilva 17:6f85605f793d 9 * copies of the Software, and to permit persons to whom the Software is
marcodesilva 17:6f85605f793d 10 * furnished to do so, subject to the following conditions:
marcodesilva 17:6f85605f793d 11 *
marcodesilva 17:6f85605f793d 12 * The above copyright notice and this permission notice shall be included in
marcodesilva 17:6f85605f793d 13 * all copies or substantial portions of the Software.
marcodesilva 17:6f85605f793d 14 *
marcodesilva 17:6f85605f793d 15 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
marcodesilva 17:6f85605f793d 16 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
marcodesilva 17:6f85605f793d 17 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
marcodesilva 17:6f85605f793d 18 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
marcodesilva 17:6f85605f793d 19 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
marcodesilva 17:6f85605f793d 20 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
marcodesilva 17:6f85605f793d 21 * THE SOFTWARE.
marcodesilva 17:6f85605f793d 22 */
marcodesilva 17:6f85605f793d 23
marcodesilva 17:6f85605f793d 24 #ifndef MBED_MX_H
marcodesilva 17:6f85605f793d 25 #define MBED_MX_H
marcodesilva 17:6f85605f793d 26
marcodesilva 17:6f85605f793d 27 #include "mbed.h"
marcodesilva 17:6f85605f793d 28 #include "UARTSerial_half.h"
marcodesilva 17:6f85605f793d 29
marcodesilva 17:6f85605f793d 30 //#define SENDPACKET_DEBUG 1
marcodesilva 17:6f85605f793d 31 //#define SENDPACKET_DEBUG_INSTRUCTION_PKT 1
marcodesilva 17:6f85605f793d 32 //#define SENDPACKET_DEBUG_STATUS_PKT 1
marcodesilva 17:6f85605f793d 33 //#define READ_DEBUG 1
marcodesilva 17:6f85605f793d 34 //#define READ_DEBUG_INSTRUCTION_PKT 1
marcodesilva 17:6f85605f793d 35 //#define READ_DEBUG_STATUS_PKT 1
marcodesilva 17:6f85605f793d 36 //#define GETPOSITION_DEBUG 1
marcodesilva 17:6f85605f793d 37
marcodesilva 17:6f85605f793d 38 //#define TORQUE_ENABLE_DEBUGDEBUG 1
marcodesilva 17:6f85605f793d 39 //#define REBOOT_ENABLE_DEBUG
marcodesilva 17:6f85605f793d 40 //#define SETGOAL_DEBUG 1
marcodesilva 17:6f85605f793d 41 //#define SETGOAL_SPEED_DEBUG 1
marcodesilva 17:6f85605f793d 42 //#define OPERATING_MODE_DEBUG 1
marcodesilva 17:6f85605f793d 43 //#define SETBAUD_DEBUG 1
marcodesilva 17:6f85605f793d 44
marcodesilva 22:5d3f37356915 45 #define SYNC_TORQUE_ENABLE_DEBUG 0
marcodesilva 22:5d3f37356915 46 #define SYNC_SETGOAL_DEBUG 0
anfontanelli 21:fe5dd48bebc6 47 #define SYNC_GET_POSITION_DEBUG 0
marcodesilva 22:5d3f37356915 48 #define SYNC_SENDPACKET_DEBUG 0
marcodesilva 22:5d3f37356915 49 #define SYNC_SENDPACKET_DEBUG_PACKETONLY 0
marcodesilva 22:5d3f37356915 50 #define SYNC_READPACKET_DEBUG_PACKETONLY 0
marcodesilva 22:5d3f37356915 51 #define SYNC_SET_BAUD_DEBUG 0
marcodesilva 22:5d3f37356915 52 #define SYNC_GET_CURRENT_DEBUG 0
marcodesilva 22:5d3f37356915 53 #define SYNC_GOAL_CURRENT_DEBUG 0
marcodesilva 22:5d3f37356915 54 #define SYNC_OPERATING_MODE_DEBUG 0
marcodesilva 17:6f85605f793d 55
marcodesilva 17:6f85605f793d 56 // Protocol 2.0
marcodesilva 17:6f85605f793d 57 // N.B. all Register values are in HEX, on the on-line manual all are in DEC
marcodesilva 17:6f85605f793d 58 #define MX_REG_ID 0x7
marcodesilva 17:6f85605f793d 59 #define MX_REG_BAUD 0x8
marcodesilva 17:6f85605f793d 60 #define MX_REG_CW_LIMIT 0x30
marcodesilva 17:6f85605f793d 61 #define MX_REG_CCW_LIMIT 0x34
marcodesilva 17:6f85605f793d 62 #define MX_REG_GOAL_POSITION 0x74
marcodesilva 17:6f85605f793d 63 #define MX_REG_GOAL_VELOCITY 0x68
marcodesilva 17:6f85605f793d 64 #define MX_REG_PRESENT_INPUT_VOLT 0x90
marcodesilva 17:6f85605f793d 65 #define MX_REG_PRESENT_TEMP 0x92
marcodesilva 17:6f85605f793d 66 #define MX_REG_MOVING 0x7A
marcodesilva 17:6f85605f793d 67 #define MX_REG_PRESENT_POSITION 0x84
marcodesilva 17:6f85605f793d 68 #define MX_REG_TORQUE_ENABLE 0x40
marcodesilva 17:6f85605f793d 69 #define MX_REG_OPERATING_MODE 0xB
marcodesilva 17:6f85605f793d 70 #define MX_REG_PRESENT_CURRENT 0x7E
marcodesilva 17:6f85605f793d 71 #define MX_REG_GOAL_CURRENT 0x66
marcodesilva 17:6f85605f793d 72 #define MX_REG_CURRENT_LIMIT 0x26
marcodesilva 19:16d5f8a3311f 73 #define MX_REG_GOAL_PWM 0x64
marcodesilva 22:5d3f37356915 74 #define MX_REG_PROFILE_ACCELER 0x6C
marcodesilva 22:5d3f37356915 75 #define MX_REG_PROFILE_VELOCITY 0x70
marcodesilva 17:6f85605f793d 76 #define MX_OFF 0
marcodesilva 17:6f85605f793d 77 #define MX_ON 1
marcodesilva 17:6f85605f793d 78
marcodesilva 17:6f85605f793d 79 class MX{
marcodesilva 17:6f85605f793d 80
marcodesilva 17:6f85605f793d 81
marcodesilva 17:6f85605f793d 82 public:
marcodesilva 17:6f85605f793d 83
marcodesilva 17:6f85605f793d 84 //standard constructor
marcodesilva 17:6f85605f793d 85 MX();
marcodesilva 17:6f85605f793d 86 //constructor
marcodesilva 17:6f85605f793d 87 MX(int*broadcastID, int Nmotor, int MotorBaud, UARTSerial_half* pCommLayer);
marcodesilva 17:6f85605f793d 88 //constructor 2
marcodesilva 17:6f85605f793d 89 MX(int ID, int MotorBaud, UARTSerial_half* pCommLayer);
marcodesilva 17:6f85605f793d 90
marcodesilva 17:6f85605f793d 91 /** Set the mode of the servo
marcodesilva 17:6f85605f793d 92 * @param mode
marcodesilva 17:6f85605f793d 93 *
marcodesilva 17:6f85605f793d 94 *
marcodesilva 17:6f85605f793d 95 */
marcodesilva 17:6f85605f793d 96 int OperatingMode (int mode);
marcodesilva 17:6f85605f793d 97
marcodesilva 17:6f85605f793d 98 /** Set baud rate of all attached servos
marcodesilva 17:6f85605f793d 99 * @param mode
marcodesilva 17:6f85605f793d 100 * 0x00 = 9,600 bps
marcodesilva 17:6f85605f793d 101 * 0x01 = 57,600 bps (DEFAULT)
marcodesilva 17:6f85605f793d 102 * 0x02 = 115,200 bps
marcodesilva 17:6f85605f793d 103 * 0x03 = 1 Mbps
marcodesilva 17:6f85605f793d 104 * 0x04 = 2 Mbps
marcodesilva 17:6f85605f793d 105 * 0x05 = 3 Mbps
marcodesilva 17:6f85605f793d 106 * 0x06 = 4 Mbps
marcodesilva 17:6f85605f793d 107 * 0x06 = 4,5 Mbps
marcodesilva 17:6f85605f793d 108 */
marcodesilva 17:6f85605f793d 109 int motorSetBaud(int MotorBaud);
marcodesilva 17:6f85605f793d 110
marcodesilva 17:6f85605f793d 111 /** Set Torque enable
marcodesilva 17:6f85605f793d 112 * enableVal = 1 Torque On
marcodesilva 17:6f85605f793d 113 * enableVal = 0 Torque Off
marcodesilva 17:6f85605f793d 114 **/
marcodesilva 17:6f85605f793d 115 int TorqueEnable(bool enable);
marcodesilva 17:6f85605f793d 116
marcodesilva 17:6f85605f793d 117 /** Set goal angle in integer degrees, in positional mode
marcodesilva 17:6f85605f793d 118 *
marcodesilva 17:6f85605f793d 119 * @param degrees 0-300
marcodesilva 17:6f85605f793d 120 * @param flags, defaults to 0
marcodesilva 17:6f85605f793d 121 * flags[0] = blocking, return when goal position reached
marcodesilva 17:6f85605f793d 122 * flags[1] = register, activate with a broadcast trigger
marcodesilva 17:6f85605f793d 123 *
marcodesilva 17:6f85605f793d 124 */
marcodesilva 17:6f85605f793d 125 int SetGoal(float degrees, int flags = 0);
marcodesilva 17:6f85605f793d 126 /** Set goal angle in integer degrees, in positional mode
marcodesilva 17:6f85605f793d 127 *
marcodesilva 17:6f85605f793d 128 * @param degrees 0-360
marcodesilva 17:6f85605f793d 129 * @param speed 0-100
marcodesilva 17:6f85605f793d 130 * @param flags, defaults to 0
marcodesilva 17:6f85605f793d 131 * flags[0] = blocking, return when goal position reached
marcodesilva 17:6f85605f793d 132 * flags[1] = register, activate with a broadcast trigger
marcodesilva 17:6f85605f793d 133 *
marcodesilva 17:6f85605f793d 134 */
marcodesilva 17:6f85605f793d 135 int SetGoalSpeed(int speed, int flags = 0);
marcodesilva 17:6f85605f793d 136
marcodesilva 17:6f85605f793d 137
marcodesilva 17:6f85605f793d 138 /** Set the speed of the servo in continuous rotation mode
marcodesilva 17:6f85605f793d 139 *
marcodesilva 17:6f85605f793d 140 * @param speed, -1.0 to 1.0
marcodesilva 17:6f85605f793d 141 * -1.0 = full speed counter clock wise
marcodesilva 17:6f85605f793d 142 * 1.0 = full speed clock wise
marcodesilva 17:6f85605f793d 143 */
marcodesilva 17:6f85605f793d 144 int SetCRSpeed(float speed);
marcodesilva 17:6f85605f793d 145
marcodesilva 17:6f85605f793d 146
marcodesilva 17:6f85605f793d 147 /** Set the clockwise limit of the servo
marcodesilva 17:6f85605f793d 148 *
marcodesilva 17:6f85605f793d 149 * @param degrees, 0-360
marcodesilva 17:6f85605f793d 150 */
marcodesilva 17:6f85605f793d 151 int SetCWLimit(int degrees);
marcodesilva 17:6f85605f793d 152
marcodesilva 17:6f85605f793d 153 /** Set the counter-clockwise limit of the servo
marcodesilva 17:6f85605f793d 154 *
marcodesilva 17:6f85605f793d 155 * @param degrees, 0-360
marcodesilva 17:6f85605f793d 156 */
marcodesilva 17:6f85605f793d 157 int SetCCWLimit(int degrees);
marcodesilva 17:6f85605f793d 158
marcodesilva 17:6f85605f793d 159 // Change the ID
marcodesilva 17:6f85605f793d 160
marcodesilva 17:6f85605f793d 161 /** Change the ID of a servo
marcodesilva 17:6f85605f793d 162 *
marcodesilva 17:6f85605f793d 163 * @param CurentID 1-255
marcodesilva 17:6f85605f793d 164 * @param NewID 1-255
marcodesilva 17:6f85605f793d 165 *
marcodesilva 17:6f85605f793d 166 * If a servo ID is not know, the broadcast address of 0 can be used for CurrentID.
marcodesilva 17:6f85605f793d 167 * In this situation, only one servo should be connected to the bus
marcodesilva 17:6f85605f793d 168 */
marcodesilva 17:6f85605f793d 169 int SetID(int CurrentID, int NewID);
marcodesilva 17:6f85605f793d 170
marcodesilva 17:6f85605f793d 171
marcodesilva 17:6f85605f793d 172 /** Poll to see if the servo is moving
marcodesilva 17:6f85605f793d 173 *
marcodesilva 17:6f85605f793d 174 * @returns true is the servo is moving
marcodesilva 17:6f85605f793d 175 */
marcodesilva 17:6f85605f793d 176 bool isMoving(void);
marcodesilva 17:6f85605f793d 177
marcodesilva 17:6f85605f793d 178 /** Send the broadcast "trigger" command, to activate any outstanding registered commands
marcodesilva 17:6f85605f793d 179 */
marcodesilva 17:6f85605f793d 180 void trigger(void);
marcodesilva 17:6f85605f793d 181
marcodesilva 17:6f85605f793d 182 /** Read the current angle of the servo
marcodesilva 17:6f85605f793d 183 *
marcodesilva 17:6f85605f793d 184 * @returns float in the range 0.0-300.0
marcodesilva 17:6f85605f793d 185 */
marcodesilva 17:6f85605f793d 186 float GetPosition();
marcodesilva 17:6f85605f793d 187
marcodesilva 17:6f85605f793d 188 /** Read the temperature of the servo
marcodesilva 17:6f85605f793d 189 *
marcodesilva 17:6f85605f793d 190 * @returns float temperature
marcodesilva 17:6f85605f793d 191 */
marcodesilva 17:6f85605f793d 192 float GetTemp(void);
marcodesilva 17:6f85605f793d 193
marcodesilva 17:6f85605f793d 194 /** Read the supply voltage of the servo
marcodesilva 17:6f85605f793d 195 *
marcodesilva 17:6f85605f793d 196 * @returns float voltage
marcodesilva 17:6f85605f793d 197 */
marcodesilva 17:6f85605f793d 198 float GetVolts(void);
marcodesilva 17:6f85605f793d 199 /** ENABLE
marcodesilva 17:6f85605f793d 200 *
marcodesilva 17:6f85605f793d 201 * @returns float voltage
marcodesilva 17:6f85605f793d 202 */
marcodesilva 17:6f85605f793d 203
marcodesilva 17:6f85605f793d 204 char* readPacket( int start, int length, int flag=0);
marcodesilva 17:6f85605f793d 205 int sendPacket(int start, int length, char* data, int flag=0);
marcodesilva 17:6f85605f793d 206 void Reboot();
marcodesilva 17:6f85605f793d 207
anfontanelli 21:fe5dd48bebc6 208 void SyncReadPacket(int start, int bytes, char* data);
anfontanelli 21:fe5dd48bebc6 209 void SyncGetPosition(float* angle);
anfontanelli 21:fe5dd48bebc6 210 void SyncGetCurrent(float* current);
marcodesilva 17:6f85605f793d 211 int SyncTorqueEnable(bool enableVal[], int Nmotor =-1);
marcodesilva 17:6f85605f793d 212 int SyncSetGoal(float degrees[]);
marcodesilva 17:6f85605f793d 213 int SyncSetGoal(float degrees, int ID);
marcodesilva 17:6f85605f793d 214 int SyncSendPacket(int start, int bytes, char* data, int ID =-1);
marcodesilva 17:6f85605f793d 215 int SyncGoalCurrent(float mAmpere, int ID);
marcodesilva 17:6f85605f793d 216 int SyncGoalCurrent(float mAmpere[]);
marcodesilva 17:6f85605f793d 217 int SyncCurrentLimit(float mAmpere, int ID);
marcodesilva 17:6f85605f793d 218 int SyncCurrentLimit(float mAmpere[]);
marcodesilva 22:5d3f37356915 219 int SyncOperatingMode(int mode[], int ID =-1);
marcodesilva 22:5d3f37356915 220 int SyncSetBaud(int MotorBaud[], int ID =-1 );
marcodesilva 22:5d3f37356915 221 int SyncGoalPWM(float values[], int ID =-1 );
marcodesilva 22:5d3f37356915 222 int SyncProfileAccel(float profileValAcc[]);
marcodesilva 22:5d3f37356915 223 int SyncProfileVel(float profileValueVel[]);
marcodesilva 17:6f85605f793d 224
marcodesilva 17:6f85605f793d 225 private :
marcodesilva 17:6f85605f793d 226
marcodesilva 17:6f85605f793d 227 UARTSerial_half* _pCommLayer;
marcodesilva 17:6f85605f793d 228 int _ID;
marcodesilva 17:6f85605f793d 229 int _MotorBaud;
marcodesilva 17:6f85605f793d 230 int* _broadcastID;
marcodesilva 17:6f85605f793d 231 int _Nmotor;
marcodesilva 17:6f85605f793d 232
marcodesilva 17:6f85605f793d 233 unsigned short update_crc(unsigned short crc_accum, char *data_blk_ptr, unsigned short data_blk_size);
marcodesilva 17:6f85605f793d 234 };
marcodesilva 17:6f85605f793d 235
marcodesilva 17:6f85605f793d 236 #endif