hisyam fs / Mbed 2 deprecated Test_servo

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.

Dependencies:   mbed

AX12/AX12.h@1:2759fc78aa04, 2018-08-31 (annotated)

Committer:
hisyamfs
Date:
Fri Aug 31 10:45:30 2018 +0000
Revision:
1:2759fc78aa04
Parent:
0:23b97841e8dc 
Bisa;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
hisyamfs 0:23b97841e8dc 1 /* mbed AX-12+ Servo Library
hisyamfs 0:23b97841e8dc 2 *
hisyamfs 0:23b97841e8dc 3 * Copyright (c) 2010, cstyles (http://mbed.org)
hisyamfs 0:23b97841e8dc 4 *
hisyamfs 0:23b97841e8dc 5 * Permission is hereby granted, free of charge, to any person obtaining a copy
hisyamfs 0:23b97841e8dc 6 * of this software and associated documentation files (the "Software"), to deal
hisyamfs 0:23b97841e8dc 7 * in the Software without restriction, including without limitation the rights
hisyamfs 0:23b97841e8dc 8 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
hisyamfs 0:23b97841e8dc 9 * copies of the Software, and to permit persons to whom the Software is
hisyamfs 0:23b97841e8dc 10 * furnished to do so, subject to the following conditions:
hisyamfs 0:23b97841e8dc 11 *
hisyamfs 0:23b97841e8dc 12 * The above copyright notice and this permission notice shall be included in
hisyamfs 0:23b97841e8dc 13 * all copies or substantial portions of the Software.
hisyamfs 0:23b97841e8dc 14 *
hisyamfs 0:23b97841e8dc 15 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
hisyamfs 0:23b97841e8dc 16 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
hisyamfs 0:23b97841e8dc 17 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
hisyamfs 0:23b97841e8dc 18 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
hisyamfs 0:23b97841e8dc 19 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
hisyamfs 0:23b97841e8dc 20 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
hisyamfs 0:23b97841e8dc 21 * THE SOFTWARE.
hisyamfs 0:23b97841e8dc 22 */
hisyamfs 0:23b97841e8dc 23
hisyamfs 0:23b97841e8dc 24 #ifndef MBED_AX12_H
hisyamfs 0:23b97841e8dc 25 #define MBED_AX12_H
hisyamfs 0:23b97841e8dc 26
hisyamfs 0:23b97841e8dc 27 #include "mbed.h"
hisyamfs 0:23b97841e8dc 28
hisyamfs 0:23b97841e8dc 29 #define AX12_REG_ID 0x3
hisyamfs 0:23b97841e8dc 30 #define AX12_REG_BAUD 0x4
hisyamfs 0:23b97841e8dc 31 #define AX12_REG_CW_LIMIT 0x06
hisyamfs 0:23b97841e8dc 32 #define AX12_REG_CCW_LIMIT 0x08
hisyamfs 0:23b97841e8dc 33 #define AX12_REG_GOAL_POSITION 0x1E
hisyamfs 0:23b97841e8dc 34 #define AX12_REG_MOVING_SPEED 0x20
hisyamfs 0:23b97841e8dc 35 #define AX12_REG_VOLTS 0x2A
hisyamfs 0:23b97841e8dc 36 #define AX12_REG_TEMP 0x2B
hisyamfs 0:23b97841e8dc 37 #define AX12_REG_MOVING 0x2E
hisyamfs 0:23b97841e8dc 38 #define AX12_REG_POSITION 0x24
hisyamfs 1:2759fc78aa04 39 #define AX12_TORQUE_ENABLE 0x18
hisyamfs 0:23b97841e8dc 40
hisyamfs 0:23b97841e8dc 41 #define AX12_MODE_POSITION 0
hisyamfs 0:23b97841e8dc 42 #define AX12_MODE_ROTATION 1
hisyamfs 0:23b97841e8dc 43
hisyamfs 0:23b97841e8dc 44 #define AX12_CW 1
hisyamfs 0:23b97841e8dc 45 #define AX12_CCW 0
hisyamfs 0:23b97841e8dc 46
hisyamfs 0:23b97841e8dc 47 /** Servo control class for AX12 and other compatible Dynamixel servo
hisyamfs 0:23b97841e8dc 48 *
hisyamfs 0:23b97841e8dc 49 * Example:
hisyamfs 0:23b97841e8dc 50 * @code
hisyamfs 0:23b97841e8dc 51 * #include "mbed.h"
hisyamfs 0:23b97841e8dc 52 * #include "AX12.h"
hisyamfs 0:23b97841e8dc 53 *
hisyamfs 0:23b97841e8dc 54 * int main() {
hisyamfs 0:23b97841e8dc 55 * // The controlled servo has ID = 1
hisyamfs 0:23b97841e8dc 56 * // Connected to pin p9 as TX and p10 as RX
hisyamfs 0:23b97841e8dc 57 * // p11 used as toggle between TX and RX mode
hisyamfs 0:23b97841e8dc 58 * // The baudrate used to communicate with servo is 1 000 000
hisyamfs 0:23b97841e8dc 59 * AX12 myax12 (p9, p10, p11, 1, 1000000);
hisyamfs 0:23b97841e8dc 60 *
hisyamfs 0:23b97841e8dc 61 * while (1) {
hisyamfs 0:23b97841e8dc 62 * myax12.SetGoal(0); // go to 0 degrees
hisyamfs 0:23b97841e8dc 63 * wait (2.0);
hisyamfs 0:23b97841e8dc 64 * myax12.SetGoal(300); // go to 300 degrees
hisyamfs 0:23b97841e8dc 65 * wait (2.0);
hisyamfs 0:23b97841e8dc 66 * }
hisyamfs 0:23b97841e8dc 67 * }
hisyamfs 0:23b97841e8dc 68 * @endcode
hisyamfs 0:23b97841e8dc 69 */
hisyamfs 0:23b97841e8dc 70 class AX12 {
hisyamfs 0:23b97841e8dc 71
hisyamfs 0:23b97841e8dc 72 public:
hisyamfs 0:23b97841e8dc 73
hisyamfs 0:23b97841e8dc 74 /** Create an AX12 servo object connected to the specified serial port, with the specified ID
hisyamfs 0:23b97841e8dc 75 *
hisyamfs 0:23b97841e8dc 76 * @param pin tx pin
hisyamfs 0:23b97841e8dc 77 * @param pin rx pin
hisyamfs 0:23b97841e8dc 78 * @param pin tx enable, to switch between tx and rx
hisyamfs 0:23b97841e8dc 79 * @param int ID, the Bus ID of the servo 1-255
hisyamfs 0:23b97841e8dc 80 * @param int baud, the baudrate of the servo
hisyamfs 0:23b97841e8dc 81 */
hisyamfs 0:23b97841e8dc 82 AX12(PinName tx, PinName rx, PinName tx_ena, int ID, int baud);
hisyamfs 0:23b97841e8dc 83
hisyamfs 0:23b97841e8dc 84 /** Set the mode of the servo
hisyamfs 0:23b97841e8dc 85 * @param mode
hisyamfs 0:23b97841e8dc 86 * 0 = Positional, default
hisyamfs 0:23b97841e8dc 87 * 1 = Continuous rotation
hisyamfs 0:23b97841e8dc 88 */
hisyamfs 0:23b97841e8dc 89 int SetMode(int mode);
hisyamfs 0:23b97841e8dc 90
hisyamfs 0:23b97841e8dc 91 /** Set baud rate of all attached servos
hisyamfs 0:23b97841e8dc 92 * @param mode
hisyamfs 0:23b97841e8dc 93 * 0x01 = 1,000,000 bps
hisyamfs 0:23b97841e8dc 94 * 0x03 = 500,000 bps
hisyamfs 0:23b97841e8dc 95 * 0x04 = 400,000 bps
hisyamfs 0:23b97841e8dc 96 * 0x07 = 250,000 bps
hisyamfs 0:23b97841e8dc 97 * 0x09 = 200,000 bps
hisyamfs 0:23b97841e8dc 98 * 0x10 = 115,200 bps
hisyamfs 0:23b97841e8dc 99 * 0x22 = 57,600 bps
hisyamfs 0:23b97841e8dc 100 * 0x67 = 19,200 bps
hisyamfs 0:23b97841e8dc 101 * 0xCF = 9,600 bp
hisyamfs 0:23b97841e8dc 102 */
hisyamfs 0:23b97841e8dc 103 int SetBaud(int baud);
hisyamfs 0:23b97841e8dc 104
hisyamfs 0:23b97841e8dc 105 /** Set goal angle in integer degrees, in positional mode
hisyamfs 0:23b97841e8dc 106 *
hisyamfs 0:23b97841e8dc 107 * @param degrees 0-300
hisyamfs 0:23b97841e8dc 108 * @param flags, defaults to 0
hisyamfs 0:23b97841e8dc 109 * flags[0] = blocking, return when goal position reached
hisyamfs 0:23b97841e8dc 110 * flags[1] = register, activate with a broadcast trigger
hisyamfs 0:23b97841e8dc 111 *
hisyamfs 0:23b97841e8dc 112 */
hisyamfs 0:23b97841e8dc 113 int SetGoal(int degrees, int flags = 0);
hisyamfs 0:23b97841e8dc 114
hisyamfs 0:23b97841e8dc 115 /** Set the position and speed of 18 servo from ID 1-18 at the same time
hisyamfs 0:23b97841e8dc 116 *
hisyamfs 0:23b97841e8dc 117 * @param degrees1..18 The position of servo with ID 1..18
hisyamfs 0:23b97841e8dc 118 * @param speed1..18 The speed of servo with ID 1..18
hisyamfs 0:23b97841e8dc 119 *
hisyamfs 0:23b97841e8dc 120 */
hisyamfs 0:23b97841e8dc 121 void MultSetGoal(int degrees1, float speed1,
hisyamfs 0:23b97841e8dc 122 int degrees2, float speed2,
hisyamfs 0:23b97841e8dc 123 int degrees3, float speed3,
hisyamfs 0:23b97841e8dc 124 int degrees4, float speed4,
hisyamfs 0:23b97841e8dc 125 int degrees5, float speed5,
hisyamfs 0:23b97841e8dc 126 int degrees6, float speed6,
hisyamfs 0:23b97841e8dc 127 int degrees7, float speed7,
hisyamfs 0:23b97841e8dc 128 int degrees8, float speed8,
hisyamfs 0:23b97841e8dc 129 int degrees9, float speed9,
hisyamfs 0:23b97841e8dc 130 int degrees10, float speed10,
hisyamfs 0:23b97841e8dc 131 int degrees11, float speed11,
hisyamfs 0:23b97841e8dc 132 int degrees12, float speed12,
hisyamfs 0:23b97841e8dc 133 int degrees13, float speed13,
hisyamfs 0:23b97841e8dc 134 int degrees14, float speed14,
hisyamfs 0:23b97841e8dc 135 int degrees15, float speed15,
hisyamfs 0:23b97841e8dc 136 int degrees16, float speed16,
hisyamfs 0:23b97841e8dc 137 int degrees17, float speed17,
hisyamfs 0:23b97841e8dc 138 int degrees18, float speed18);
hisyamfs 0:23b97841e8dc 139
hisyamfs 0:23b97841e8dc 140 /** Set the speed of the servo in continuous rotation mode
hisyamfs 0:23b97841e8dc 141 *
hisyamfs 0:23b97841e8dc 142 * @param speed, -1.0 to 1.0
hisyamfs 0:23b97841e8dc 143 * -1.0 = full speed counter clock wise
hisyamfs 0:23b97841e8dc 144 * 1.0 = full speed clock wise
hisyamfs 0:23b97841e8dc 145 */
hisyamfs 0:23b97841e8dc 146 int SetCRSpeed(float speed);
hisyamfs 0:23b97841e8dc 147
hisyamfs 0:23b97841e8dc 148 /** Set the clockwise limit of the servo
hisyamfs 0:23b97841e8dc 149 *
hisyamfs 0:23b97841e8dc 150 * @param degrees, 0-300
hisyamfs 0:23b97841e8dc 151 */
hisyamfs 0:23b97841e8dc 152 int SetCWLimit(int degrees);
hisyamfs 0:23b97841e8dc 153
hisyamfs 0:23b97841e8dc 154 /** Set the counter-clockwise limit of the servo
hisyamfs 0:23b97841e8dc 155 *
hisyamfs 0:23b97841e8dc 156 * @param degrees, 0-300
hisyamfs 0:23b97841e8dc 157 */
hisyamfs 0:23b97841e8dc 158 int SetCCWLimit(int degrees);
hisyamfs 0:23b97841e8dc 159
hisyamfs 0:23b97841e8dc 160 /** Change the ID of a servo controlled
hisyamfs 0:23b97841e8dc 161 *
hisyamfs 0:23b97841e8dc 162 * @param ID 1-255
hisyamfs 0:23b97841e8dc 163 *
hisyamfs 0:23b97841e8dc 164 * If a servo ID is not know, the broadcast address of 0 can be used for CurrentID.
hisyamfs 0:23b97841e8dc 165 * In this situation, only one servo should be connected to the bus
hisyamfs 0:23b97841e8dc 166 */
hisyamfs 0:23b97841e8dc 167 void ControlID (int ID);
hisyamfs 0:23b97841e8dc 168
hisyamfs 0:23b97841e8dc 169 /** Change the ID of a servo
hisyamfs 0:23b97841e8dc 170 *
hisyamfs 0:23b97841e8dc 171 * @param CurentID 0-253
hisyamfs 0:23b97841e8dc 172 * @param NewID 0-253
hisyamfs 0:23b97841e8dc 173 *
hisyamfs 0:23b97841e8dc 174 * If a servo ID is not know, the broadcast address of 254 (0xFE) can be used for CurrentID.
hisyamfs 0:23b97841e8dc 175 * In this situation, only one servo should be connected to the bus
hisyamfs 0:23b97841e8dc 176 */
hisyamfs 0:23b97841e8dc 177 int SetID(int CurrentID, int NewID);
hisyamfs 0:23b97841e8dc 178
hisyamfs 0:23b97841e8dc 179
hisyamfs 0:23b97841e8dc 180 /** Poll to see if the servo is moving
hisyamfs 0:23b97841e8dc 181 *
hisyamfs 0:23b97841e8dc 182 * @returns true is the servo is moving
hisyamfs 0:23b97841e8dc 183 */
hisyamfs 0:23b97841e8dc 184 int isMoving(void);
hisyamfs 0:23b97841e8dc 185
hisyamfs 0:23b97841e8dc 186
hisyamfs 0:23b97841e8dc 187 /** Send the broadcast "trigger" command, to activate any outstanding registered commands
hisyamfs 0:23b97841e8dc 188 */
hisyamfs 0:23b97841e8dc 189 void trigger(void);
hisyamfs 0:23b97841e8dc 190
hisyamfs 0:23b97841e8dc 191
hisyamfs 0:23b97841e8dc 192 /** Read the current angle of the servo
hisyamfs 0:23b97841e8dc 193 *
hisyamfs 0:23b97841e8dc 194 * @returns float in the range 0.0-300.0
hisyamfs 0:23b97841e8dc 195 */
hisyamfs 0:23b97841e8dc 196 float GetPosition();
hisyamfs 0:23b97841e8dc 197
hisyamfs 0:23b97841e8dc 198
hisyamfs 0:23b97841e8dc 199 /** Read the temperature of the servo
hisyamfs 0:23b97841e8dc 200 *
hisyamfs 0:23b97841e8dc 201 * @returns float temperature
hisyamfs 0:23b97841e8dc 202 */
hisyamfs 0:23b97841e8dc 203 float GetTemp(void);
hisyamfs 0:23b97841e8dc 204
hisyamfs 0:23b97841e8dc 205
hisyamfs 0:23b97841e8dc 206 /** Read the supply voltage of the servo
hisyamfs 0:23b97841e8dc 207 *
hisyamfs 0:23b97841e8dc 208 * @returns float voltage
hisyamfs 0:23b97841e8dc 209 */
hisyamfs 0:23b97841e8dc 210 float GetVolts(void);
hisyamfs 1:2759fc78aa04 211
hisyamfs 1:2759fc78aa04 212 /** Set torque enable of the servo
hisyamfs 1:2759fc78aa04 213 */
hisyamfs 1:2759fc78aa04 214 void SetTorqueEnable(char val);
hisyamfs 0:23b97841e8dc 215
hisyamfs 0:23b97841e8dc 216 int read(int ID, int start, int length, char* data);
hisyamfs 0:23b97841e8dc 217
hisyamfs 0:23b97841e8dc 218 int write(int ID, int start, int length, char* data, int flag=0);
hisyamfs 0:23b97841e8dc 219
hisyamfs 0:23b97841e8dc 220 int sync_write(char* data);
hisyamfs 0:23b97841e8dc 221
hisyamfs 0:23b97841e8dc 222 private :
hisyamfs 0:23b97841e8dc 223
hisyamfs 0:23b97841e8dc 224 Serial _ax12;
hisyamfs 0:23b97841e8dc 225 DigitalOut _ena_tx;
hisyamfs 0:23b97841e8dc 226 char TxBuf[10];
hisyamfs 0:23b97841e8dc 227 char Status[10];
hisyamfs 0:23b97841e8dc 228 static const char start_reg = 0x1E;
hisyamfs 0:23b97841e8dc 229 static const char length_data = 4;
hisyamfs 0:23b97841e8dc 230 static const char total_ID = 18;
hisyamfs 0:23b97841e8dc 231 int _ID;
hisyamfs 0:23b97841e8dc 232 int _baud_bit;
hisyamfs 0:23b97841e8dc 233 int _wait_sent;
hisyamfs 0:23b97841e8dc 234 int _timeout_tick;
hisyamfs 0:23b97841e8dc 235 int _error_code;
hisyamfs 0:23b97841e8dc 236 char _data[2];
hisyamfs 0:23b97841e8dc 237 char _reg_flag;
hisyamfs 0:23b97841e8dc 238 };
hisyamfs 0:23b97841e8dc 239
hisyamfs 0:23b97841e8dc 240 #endif