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@0:23b97841e8dc, 2018-08-29 (annotated)

Committer:
hisyamfs
Date:
Wed Aug 29 09:45:32 2018 +0000
Revision:
0:23b97841e8dc
Child:
1:2759fc78aa04
2 objek

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 0:23b97841e8dc 39
hisyamfs 0:23b97841e8dc 40 #define AX12_MODE_POSITION 0
hisyamfs 0:23b97841e8dc 41 #define AX12_MODE_ROTATION 1
hisyamfs 0:23b97841e8dc 42
hisyamfs 0:23b97841e8dc 43 #define AX12_CW 1
hisyamfs 0:23b97841e8dc 44 #define AX12_CCW 0
hisyamfs 0:23b97841e8dc 45
hisyamfs 0:23b97841e8dc 46 /** Servo control class for AX12 and other compatible Dynamixel servo
hisyamfs 0:23b97841e8dc 47 *
hisyamfs 0:23b97841e8dc 48 * Example:
hisyamfs 0:23b97841e8dc 49 * @code
hisyamfs 0:23b97841e8dc 50 * #include "mbed.h"
hisyamfs 0:23b97841e8dc 51 * #include "AX12.h"
hisyamfs 0:23b97841e8dc 52 *
hisyamfs 0:23b97841e8dc 53 * int main() {
hisyamfs 0:23b97841e8dc 54 * // The controlled servo has ID = 1
hisyamfs 0:23b97841e8dc 55 * // Connected to pin p9 as TX and p10 as RX
hisyamfs 0:23b97841e8dc 56 * // p11 used as toggle between TX and RX mode
hisyamfs 0:23b97841e8dc 57 * // The baudrate used to communicate with servo is 1 000 000
hisyamfs 0:23b97841e8dc 58 * AX12 myax12 (p9, p10, p11, 1, 1000000);
hisyamfs 0:23b97841e8dc 59 *
hisyamfs 0:23b97841e8dc 60 * while (1) {
hisyamfs 0:23b97841e8dc 61 * myax12.SetGoal(0); // go to 0 degrees
hisyamfs 0:23b97841e8dc 62 * wait (2.0);
hisyamfs 0:23b97841e8dc 63 * myax12.SetGoal(300); // go to 300 degrees
hisyamfs 0:23b97841e8dc 64 * wait (2.0);
hisyamfs 0:23b97841e8dc 65 * }
hisyamfs 0:23b97841e8dc 66 * }
hisyamfs 0:23b97841e8dc 67 * @endcode
hisyamfs 0:23b97841e8dc 68 */
hisyamfs 0:23b97841e8dc 69 class AX12 {
hisyamfs 0:23b97841e8dc 70
hisyamfs 0:23b97841e8dc 71 public:
hisyamfs 0:23b97841e8dc 72
hisyamfs 0:23b97841e8dc 73 /** Create an AX12 servo object connected to the specified serial port, with the specified ID
hisyamfs 0:23b97841e8dc 74 *
hisyamfs 0:23b97841e8dc 75 * @param pin tx pin
hisyamfs 0:23b97841e8dc 76 * @param pin rx pin
hisyamfs 0:23b97841e8dc 77 * @param pin tx enable, to switch between tx and rx
hisyamfs 0:23b97841e8dc 78 * @param int ID, the Bus ID of the servo 1-255
hisyamfs 0:23b97841e8dc 79 * @param int baud, the baudrate of the servo
hisyamfs 0:23b97841e8dc 80 */
hisyamfs 0:23b97841e8dc 81 AX12(PinName tx, PinName rx, PinName tx_ena, int ID, int baud);
hisyamfs 0:23b97841e8dc 82
hisyamfs 0:23b97841e8dc 83 /** Set the mode of the servo
hisyamfs 0:23b97841e8dc 84 * @param mode
hisyamfs 0:23b97841e8dc 85 * 0 = Positional, default
hisyamfs 0:23b97841e8dc 86 * 1 = Continuous rotation
hisyamfs 0:23b97841e8dc 87 */
hisyamfs 0:23b97841e8dc 88 int SetMode(int mode);
hisyamfs 0:23b97841e8dc 89
hisyamfs 0:23b97841e8dc 90 /** Set baud rate of all attached servos
hisyamfs 0:23b97841e8dc 91 * @param mode
hisyamfs 0:23b97841e8dc 92 * 0x01 = 1,000,000 bps
hisyamfs 0:23b97841e8dc 93 * 0x03 = 500,000 bps
hisyamfs 0:23b97841e8dc 94 * 0x04 = 400,000 bps
hisyamfs 0:23b97841e8dc 95 * 0x07 = 250,000 bps
hisyamfs 0:23b97841e8dc 96 * 0x09 = 200,000 bps
hisyamfs 0:23b97841e8dc 97 * 0x10 = 115,200 bps
hisyamfs 0:23b97841e8dc 98 * 0x22 = 57,600 bps
hisyamfs 0:23b97841e8dc 99 * 0x67 = 19,200 bps
hisyamfs 0:23b97841e8dc 100 * 0xCF = 9,600 bp
hisyamfs 0:23b97841e8dc 101 */
hisyamfs 0:23b97841e8dc 102 int SetBaud(int baud);
hisyamfs 0:23b97841e8dc 103
hisyamfs 0:23b97841e8dc 104 /** Set goal angle in integer degrees, in positional mode
hisyamfs 0:23b97841e8dc 105 *
hisyamfs 0:23b97841e8dc 106 * @param degrees 0-300
hisyamfs 0:23b97841e8dc 107 * @param flags, defaults to 0
hisyamfs 0:23b97841e8dc 108 * flags[0] = blocking, return when goal position reached
hisyamfs 0:23b97841e8dc 109 * flags[1] = register, activate with a broadcast trigger
hisyamfs 0:23b97841e8dc 110 *
hisyamfs 0:23b97841e8dc 111 */
hisyamfs 0:23b97841e8dc 112 int SetGoal(int degrees, int flags = 0);
hisyamfs 0:23b97841e8dc 113
hisyamfs 0:23b97841e8dc 114 /** Set the position and speed of 18 servo from ID 1-18 at the same time
hisyamfs 0:23b97841e8dc 115 *
hisyamfs 0:23b97841e8dc 116 * @param degrees1..18 The position of servo with ID 1..18
hisyamfs 0:23b97841e8dc 117 * @param speed1..18 The speed of servo with ID 1..18
hisyamfs 0:23b97841e8dc 118 *
hisyamfs 0:23b97841e8dc 119 */
hisyamfs 0:23b97841e8dc 120 void MultSetGoal(int degrees1, float speed1,
hisyamfs 0:23b97841e8dc 121 int degrees2, float speed2,
hisyamfs 0:23b97841e8dc 122 int degrees3, float speed3,
hisyamfs 0:23b97841e8dc 123 int degrees4, float speed4,
hisyamfs 0:23b97841e8dc 124 int degrees5, float speed5,
hisyamfs 0:23b97841e8dc 125 int degrees6, float speed6,
hisyamfs 0:23b97841e8dc 126 int degrees7, float speed7,
hisyamfs 0:23b97841e8dc 127 int degrees8, float speed8,
hisyamfs 0:23b97841e8dc 128 int degrees9, float speed9,
hisyamfs 0:23b97841e8dc 129 int degrees10, float speed10,
hisyamfs 0:23b97841e8dc 130 int degrees11, float speed11,
hisyamfs 0:23b97841e8dc 131 int degrees12, float speed12,
hisyamfs 0:23b97841e8dc 132 int degrees13, float speed13,
hisyamfs 0:23b97841e8dc 133 int degrees14, float speed14,
hisyamfs 0:23b97841e8dc 134 int degrees15, float speed15,
hisyamfs 0:23b97841e8dc 135 int degrees16, float speed16,
hisyamfs 0:23b97841e8dc 136 int degrees17, float speed17,
hisyamfs 0:23b97841e8dc 137 int degrees18, float speed18);
hisyamfs 0:23b97841e8dc 138
hisyamfs 0:23b97841e8dc 139 /** Set the speed of the servo in continuous rotation mode
hisyamfs 0:23b97841e8dc 140 *
hisyamfs 0:23b97841e8dc 141 * @param speed, -1.0 to 1.0
hisyamfs 0:23b97841e8dc 142 * -1.0 = full speed counter clock wise
hisyamfs 0:23b97841e8dc 143 * 1.0 = full speed clock wise
hisyamfs 0:23b97841e8dc 144 */
hisyamfs 0:23b97841e8dc 145 int SetCRSpeed(float speed);
hisyamfs 0:23b97841e8dc 146
hisyamfs 0:23b97841e8dc 147 /** Set the clockwise limit of the servo
hisyamfs 0:23b97841e8dc 148 *
hisyamfs 0:23b97841e8dc 149 * @param degrees, 0-300
hisyamfs 0:23b97841e8dc 150 */
hisyamfs 0:23b97841e8dc 151 int SetCWLimit(int degrees);
hisyamfs 0:23b97841e8dc 152
hisyamfs 0:23b97841e8dc 153 /** Set the counter-clockwise limit of the servo
hisyamfs 0:23b97841e8dc 154 *
hisyamfs 0:23b97841e8dc 155 * @param degrees, 0-300
hisyamfs 0:23b97841e8dc 156 */
hisyamfs 0:23b97841e8dc 157 int SetCCWLimit(int degrees);
hisyamfs 0:23b97841e8dc 158
hisyamfs 0:23b97841e8dc 159 /** Change the ID of a servo controlled
hisyamfs 0:23b97841e8dc 160 *
hisyamfs 0:23b97841e8dc 161 * @param ID 1-255
hisyamfs 0:23b97841e8dc 162 *
hisyamfs 0:23b97841e8dc 163 * If a servo ID is not know, the broadcast address of 0 can be used for CurrentID.
hisyamfs 0:23b97841e8dc 164 * In this situation, only one servo should be connected to the bus
hisyamfs 0:23b97841e8dc 165 */
hisyamfs 0:23b97841e8dc 166 void ControlID (int ID);
hisyamfs 0:23b97841e8dc 167
hisyamfs 0:23b97841e8dc 168 /** Change the ID of a servo
hisyamfs 0:23b97841e8dc 169 *
hisyamfs 0:23b97841e8dc 170 * @param CurentID 0-253
hisyamfs 0:23b97841e8dc 171 * @param NewID 0-253
hisyamfs 0:23b97841e8dc 172 *
hisyamfs 0:23b97841e8dc 173 * If a servo ID is not know, the broadcast address of 254 (0xFE) can be used for CurrentID.
hisyamfs 0:23b97841e8dc 174 * In this situation, only one servo should be connected to the bus
hisyamfs 0:23b97841e8dc 175 */
hisyamfs 0:23b97841e8dc 176 int SetID(int CurrentID, int NewID);
hisyamfs 0:23b97841e8dc 177
hisyamfs 0:23b97841e8dc 178
hisyamfs 0:23b97841e8dc 179 /** Poll to see if the servo is moving
hisyamfs 0:23b97841e8dc 180 *
hisyamfs 0:23b97841e8dc 181 * @returns true is the servo is moving
hisyamfs 0:23b97841e8dc 182 */
hisyamfs 0:23b97841e8dc 183 int isMoving(void);
hisyamfs 0:23b97841e8dc 184
hisyamfs 0:23b97841e8dc 185
hisyamfs 0:23b97841e8dc 186 /** Send the broadcast "trigger" command, to activate any outstanding registered commands
hisyamfs 0:23b97841e8dc 187 */
hisyamfs 0:23b97841e8dc 188 void trigger(void);
hisyamfs 0:23b97841e8dc 189
hisyamfs 0:23b97841e8dc 190
hisyamfs 0:23b97841e8dc 191 /** Read the current angle of the servo
hisyamfs 0:23b97841e8dc 192 *
hisyamfs 0:23b97841e8dc 193 * @returns float in the range 0.0-300.0
hisyamfs 0:23b97841e8dc 194 */
hisyamfs 0:23b97841e8dc 195 float GetPosition();
hisyamfs 0:23b97841e8dc 196
hisyamfs 0:23b97841e8dc 197
hisyamfs 0:23b97841e8dc 198 /** Read the temperature of the servo
hisyamfs 0:23b97841e8dc 199 *
hisyamfs 0:23b97841e8dc 200 * @returns float temperature
hisyamfs 0:23b97841e8dc 201 */
hisyamfs 0:23b97841e8dc 202 float GetTemp(void);
hisyamfs 0:23b97841e8dc 203
hisyamfs 0:23b97841e8dc 204
hisyamfs 0:23b97841e8dc 205 /** Read the supply voltage of the servo
hisyamfs 0:23b97841e8dc 206 *
hisyamfs 0:23b97841e8dc 207 * @returns float voltage
hisyamfs 0:23b97841e8dc 208 */
hisyamfs 0:23b97841e8dc 209 float GetVolts(void);
hisyamfs 0:23b97841e8dc 210
hisyamfs 0:23b97841e8dc 211 int read(int ID, int start, int length, char* data);
hisyamfs 0:23b97841e8dc 212
hisyamfs 0:23b97841e8dc 213 int write(int ID, int start, int length, char* data, int flag=0);
hisyamfs 0:23b97841e8dc 214
hisyamfs 0:23b97841e8dc 215 int sync_write(char* data);
hisyamfs 0:23b97841e8dc 216
hisyamfs 0:23b97841e8dc 217 private :
hisyamfs 0:23b97841e8dc 218
hisyamfs 0:23b97841e8dc 219 Serial _ax12;
hisyamfs 0:23b97841e8dc 220 DigitalOut _ena_tx;
hisyamfs 0:23b97841e8dc 221 char TxBuf[10];
hisyamfs 0:23b97841e8dc 222 char Status[10];
hisyamfs 0:23b97841e8dc 223 static const char start_reg = 0x1E;
hisyamfs 0:23b97841e8dc 224 static const char length_data = 4;
hisyamfs 0:23b97841e8dc 225 static const char total_ID = 18;
hisyamfs 0:23b97841e8dc 226 int _ID;
hisyamfs 0:23b97841e8dc 227 int _baud_bit;
hisyamfs 0:23b97841e8dc 228 int _wait_sent;
hisyamfs 0:23b97841e8dc 229 int _timeout_tick;
hisyamfs 0:23b97841e8dc 230 int _error_code;
hisyamfs 0:23b97841e8dc 231 char _data[2];
hisyamfs 0:23b97841e8dc 232 char _reg_flag;
hisyamfs 0:23b97841e8dc 233 };
hisyamfs 0:23b97841e8dc 234
hisyamfs 0:23b97841e8dc 235 #endif