config AX12
Fork of configure_ax12_test_bras_module by
AX12.cpp@1:fce3323d874f, 2016-11-25 (annotated)
- Committer:
- slowness
- Date:
- Fri Nov 25 17:38:14 2016 +0000
- Revision:
- 1:fce3323d874f
- Parent:
- 0:c03cffe402df
mouvement du bras
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
slowness | 0:c03cffe402df | 1 | /* mbed AX-12+ Servo Library |
slowness | 0:c03cffe402df | 2 | * |
slowness | 0:c03cffe402df | 3 | * Copyright (c) 2010, cstyles (http://mbed.org) |
slowness | 0:c03cffe402df | 4 | * |
slowness | 0:c03cffe402df | 5 | * Permission is hereby granted, free of charge, to any person obtaining a copy |
slowness | 0:c03cffe402df | 6 | * of this software and associated documentation files (the "Software"), to deal |
slowness | 0:c03cffe402df | 7 | * in the Software without restriction, including without limitation the rights |
slowness | 0:c03cffe402df | 8 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell |
slowness | 0:c03cffe402df | 9 | * copies of the Software, and to permit persons to whom the Software is |
slowness | 0:c03cffe402df | 10 | * furnished to do so, subject to the following conditions: |
slowness | 0:c03cffe402df | 11 | * |
slowness | 0:c03cffe402df | 12 | * The above copyright notice and this permission notice shall be included in |
slowness | 0:c03cffe402df | 13 | * all copies or substantial portions of the Software. |
slowness | 0:c03cffe402df | 14 | * |
slowness | 0:c03cffe402df | 15 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR |
slowness | 0:c03cffe402df | 16 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, |
slowness | 0:c03cffe402df | 17 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE |
slowness | 0:c03cffe402df | 18 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER |
slowness | 0:c03cffe402df | 19 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, |
slowness | 0:c03cffe402df | 20 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN |
slowness | 0:c03cffe402df | 21 | * THE SOFTWARE. |
slowness | 0:c03cffe402df | 22 | */ |
slowness | 0:c03cffe402df | 23 | |
slowness | 0:c03cffe402df | 24 | #include "mbed.h" |
slowness | 0:c03cffe402df | 25 | #include "AX12.h" |
slowness | 0:c03cffe402df | 26 | |
slowness | 0:c03cffe402df | 27 | #define MAX_TIMEOUT 5000 |
slowness | 0:c03cffe402df | 28 | |
slowness | 0:c03cffe402df | 29 | extern Timer t; |
slowness | 0:c03cffe402df | 30 | |
slowness | 0:c03cffe402df | 31 | typedef struct |
slowness | 0:c03cffe402df | 32 | { |
slowness | 0:c03cffe402df | 33 | unsigned short Model_Number; |
slowness | 0:c03cffe402df | 34 | unsigned char Firmware; |
slowness | 0:c03cffe402df | 35 | unsigned char ID; |
slowness | 0:c03cffe402df | 36 | unsigned char Baud_Rate; |
slowness | 0:c03cffe402df | 37 | unsigned char Return_Delay_Time; |
slowness | 0:c03cffe402df | 38 | unsigned short CW_Angle_Limit; |
slowness | 0:c03cffe402df | 39 | unsigned short CCW_Angle_Limit; |
slowness | 0:c03cffe402df | 40 | unsigned char Reserved1; |
slowness | 0:c03cffe402df | 41 | unsigned char Highest_Limit_Temperature; |
slowness | 0:c03cffe402df | 42 | unsigned char Lowest_Limit_voltage; |
slowness | 0:c03cffe402df | 43 | unsigned char Highest_Limit_voltage; |
slowness | 0:c03cffe402df | 44 | unsigned short Max_Torque; |
slowness | 0:c03cffe402df | 45 | unsigned char Status_Return_Level; |
slowness | 0:c03cffe402df | 46 | unsigned char Alarm_LED; |
slowness | 0:c03cffe402df | 47 | unsigned char Alarm_Shutdown; |
slowness | 0:c03cffe402df | 48 | unsigned char Reserved2; |
slowness | 0:c03cffe402df | 49 | unsigned short Down_Calibration; |
slowness | 0:c03cffe402df | 50 | unsigned short Up_Calibration; |
slowness | 0:c03cffe402df | 51 | unsigned char Torque_Enable; |
slowness | 0:c03cffe402df | 52 | unsigned char LED; |
slowness | 0:c03cffe402df | 53 | unsigned char CW_Compliance_Margin; |
slowness | 0:c03cffe402df | 54 | unsigned char CCW_Compliance_Margin; |
slowness | 0:c03cffe402df | 55 | unsigned char CW_Compliance_Slope; |
slowness | 0:c03cffe402df | 56 | unsigned char CCW_Compliance_Slope; |
slowness | 0:c03cffe402df | 57 | unsigned short Goal_Position; |
slowness | 0:c03cffe402df | 58 | unsigned short Moving_Speed; |
slowness | 0:c03cffe402df | 59 | unsigned short Torque_Limit; |
slowness | 0:c03cffe402df | 60 | unsigned short Present_Position; |
slowness | 0:c03cffe402df | 61 | unsigned short Present_Speed; |
slowness | 0:c03cffe402df | 62 | unsigned short Present_Load; |
slowness | 0:c03cffe402df | 63 | unsigned char Present_Voltage; |
slowness | 0:c03cffe402df | 64 | unsigned char Present_Temperature; |
slowness | 0:c03cffe402df | 65 | unsigned char Registered_Instruction; |
slowness | 0:c03cffe402df | 66 | unsigned char Reserved3; |
slowness | 0:c03cffe402df | 67 | unsigned char Moving; |
slowness | 0:c03cffe402df | 68 | unsigned char Lock; |
slowness | 0:c03cffe402df | 69 | unsigned short Punch; |
slowness | 0:c03cffe402df | 70 | } T_AX12; |
slowness | 0:c03cffe402df | 71 | |
slowness | 0:c03cffe402df | 72 | |
slowness | 0:c03cffe402df | 73 | AX12::AX12(PinName tx, PinName rx, int ID, int baud) |
slowness | 0:c03cffe402df | 74 | : _ax12(tx,rx) |
slowness | 0:c03cffe402df | 75 | { |
slowness | 0:c03cffe402df | 76 | _baud = baud; |
slowness | 0:c03cffe402df | 77 | _ID = ID; |
slowness | 0:c03cffe402df | 78 | _ax12.baud(_baud); |
slowness | 0:c03cffe402df | 79 | |
slowness | 0:c03cffe402df | 80 | |
slowness | 0:c03cffe402df | 81 | } |
slowness | 0:c03cffe402df | 82 | |
slowness | 0:c03cffe402df | 83 | int AX12::Set_Secure_Goal(int degres) |
slowness | 0:c03cffe402df | 84 | { |
slowness | 0:c03cffe402df | 85 | int error = 0; |
slowness | 0:c03cffe402df | 86 | int position = 0; |
slowness | 0:c03cffe402df | 87 | int difference = 0; |
slowness | 0:c03cffe402df | 88 | int timeout_secure = 0; |
slowness | 0:c03cffe402df | 89 | int autorisation = 0; |
slowness | 0:c03cffe402df | 90 | |
slowness | 0:c03cffe402df | 91 | position = Get_Position(); |
slowness | 0:c03cffe402df | 92 | error = Set_Goal(degres); |
slowness | 0:c03cffe402df | 93 | |
slowness | 0:c03cffe402df | 94 | while ((autorisation == 0) && (timeout_secure < 100) ) |
slowness | 0:c03cffe402df | 95 | { |
slowness | 0:c03cffe402df | 96 | position = Get_Position(); |
slowness | 0:c03cffe402df | 97 | //printf("position : %d", position ); |
slowness | 0:c03cffe402df | 98 | error = Set_Goal(degres); |
slowness | 0:c03cffe402df | 99 | //printf("degres : %d", degres); |
slowness | 0:c03cffe402df | 100 | difference = degres - position; |
slowness | 0:c03cffe402df | 101 | //printf ("difference : %d", difference ); |
slowness | 0:c03cffe402df | 102 | if (((difference < 2) && (difference > (-2) ))) |
slowness | 0:c03cffe402df | 103 | autorisation = 1; |
slowness | 0:c03cffe402df | 104 | |
slowness | 0:c03cffe402df | 105 | timeout_secure++; |
slowness | 0:c03cffe402df | 106 | } |
slowness | 0:c03cffe402df | 107 | |
slowness | 0:c03cffe402df | 108 | if ( timeout_secure == 100) |
slowness | 0:c03cffe402df | 109 | { |
slowness | 0:c03cffe402df | 110 | #ifdef AX12_DEBUG |
slowness | 0:c03cffe402df | 111 | printf (" timeout secure error "); |
slowness | 0:c03cffe402df | 112 | #endif |
slowness | 0:c03cffe402df | 113 | return(-1); |
slowness | 0:c03cffe402df | 114 | } |
slowness | 0:c03cffe402df | 115 | return(error); |
slowness | 0:c03cffe402df | 116 | } |
slowness | 0:c03cffe402df | 117 | |
slowness | 0:c03cffe402df | 118 | |
slowness | 0:c03cffe402df | 119 | int AX12::Get_Return_Delay_Time(void) |
slowness | 0:c03cffe402df | 120 | { |
slowness | 0:c03cffe402df | 121 | char data[1]; |
slowness | 0:c03cffe402df | 122 | int ErrorCode = read(_ID, AX12_REG_DELAY_TIME, 1, data); |
slowness | 0:c03cffe402df | 123 | int time = data[0]; |
slowness | 0:c03cffe402df | 124 | time = 2.0 * time; |
slowness | 0:c03cffe402df | 125 | return(time); |
slowness | 0:c03cffe402df | 126 | } |
slowness | 0:c03cffe402df | 127 | |
slowness | 0:c03cffe402df | 128 | |
slowness | 0:c03cffe402df | 129 | int AX12::Get_Baud_Rate(void) |
slowness | 0:c03cffe402df | 130 | { |
slowness | 0:c03cffe402df | 131 | char data[1]; |
slowness | 0:c03cffe402df | 132 | int ErrorCode = read(_ID, AX12_REG_BAUD, 1, data); |
slowness | 0:c03cffe402df | 133 | int baud = data[0]; |
slowness | 0:c03cffe402df | 134 | baud = 2000000 / ( baud + 1 ); |
slowness | 0:c03cffe402df | 135 | return(baud); |
slowness | 0:c03cffe402df | 136 | } |
slowness | 0:c03cffe402df | 137 | |
slowness | 0:c03cffe402df | 138 | |
slowness | 0:c03cffe402df | 139 | /** Reglage du courant minimum necessaire au bon fonctionnement de l'actionneur |
slowness | 0:c03cffe402df | 140 | // minimum >> Ox000 >> decimal 0 |
slowness | 0:c03cffe402df | 141 | // maximum >> 0x3FF >> decimal 1023 |
slowness | 0:c03cffe402df | 142 | // deflaut >> 0x20 >> decimal 32 |
slowness | 0:c03cffe402df | 143 | */ |
slowness | 0:c03cffe402df | 144 | int AX12::Set_Punch(int punch) |
slowness | 0:c03cffe402df | 145 | { |
slowness | 0:c03cffe402df | 146 | char data[2]; |
slowness | 0:c03cffe402df | 147 | |
slowness | 0:c03cffe402df | 148 | data[0] = punch & 0xff; // bottom 8 bits |
slowness | 0:c03cffe402df | 149 | data[1] = punch >> 8; // top 8 bits |
slowness | 0:c03cffe402df | 150 | |
slowness | 0:c03cffe402df | 151 | // write the packet, return the error code |
slowness | 0:c03cffe402df | 152 | return (write(_ID, AX12_REG_PUNCH, 2, data)); |
slowness | 0:c03cffe402df | 153 | |
slowness | 0:c03cffe402df | 154 | } |
slowness | 0:c03cffe402df | 155 | |
slowness | 0:c03cffe402df | 156 | /** Reset |
slowness | 0:c03cffe402df | 157 | */ |
slowness | 0:c03cffe402df | 158 | int AX12::Reset(int punch) |
slowness | 0:c03cffe402df | 159 | { |
slowness | 0:c03cffe402df | 160 | char data[2]; |
slowness | 0:c03cffe402df | 161 | |
slowness | 0:c03cffe402df | 162 | data[0] = punch & 0xff; // bottom 8 bits |
slowness | 0:c03cffe402df | 163 | data[1] = punch >> 8; // top 8 bits |
slowness | 0:c03cffe402df | 164 | |
slowness | 0:c03cffe402df | 165 | // write the packet, return the error code |
slowness | 0:c03cffe402df | 166 | return (write(_ID, 0x06, 1,data)); |
slowness | 0:c03cffe402df | 167 | |
slowness | 0:c03cffe402df | 168 | } |
slowness | 0:c03cffe402df | 169 | |
slowness | 0:c03cffe402df | 170 | int AX12::Get_Punch (void) |
slowness | 0:c03cffe402df | 171 | { |
slowness | 0:c03cffe402df | 172 | char data[2]; |
slowness | 0:c03cffe402df | 173 | int ErrorCode = read(_ID, AX12_REG_PUNCH, 2, data); |
slowness | 0:c03cffe402df | 174 | int punch = data[0] | (data[1]<<8); |
slowness | 0:c03cffe402df | 175 | return(punch); |
slowness | 0:c03cffe402df | 176 | } |
slowness | 0:c03cffe402df | 177 | |
slowness | 0:c03cffe402df | 178 | |
slowness | 0:c03cffe402df | 179 | int AX12::Get_Load_Direction (void) |
slowness | 0:c03cffe402df | 180 | { |
slowness | 0:c03cffe402df | 181 | char data[2]; |
slowness | 0:c03cffe402df | 182 | int ErrorCode = read(_ID, AX12_REG_PRESENT_LOAD, 2, data); |
slowness | 0:c03cffe402df | 183 | int direction = (data[1]>>2) & 0x01; |
slowness | 0:c03cffe402df | 184 | return(direction); |
slowness | 0:c03cffe402df | 185 | } |
slowness | 0:c03cffe402df | 186 | |
slowness | 0:c03cffe402df | 187 | |
slowness | 0:c03cffe402df | 188 | int AX12::Get_Load_Value (void) |
slowness | 0:c03cffe402df | 189 | { |
slowness | 0:c03cffe402df | 190 | char data[2]; |
slowness | 0:c03cffe402df | 191 | int ErrorCode = read(_ID, AX12_REG_PRESENT_LOAD, 2, data); |
slowness | 0:c03cffe402df | 192 | int Load = (data[0] | (data[1]<<8)) & 0x3FF; |
slowness | 0:c03cffe402df | 193 | return(Load); |
slowness | 0:c03cffe402df | 194 | } |
slowness | 0:c03cffe402df | 195 | |
slowness | 0:c03cffe402df | 196 | |
slowness | 0:c03cffe402df | 197 | int AX12::Get_Present_Speed (void) |
slowness | 0:c03cffe402df | 198 | { |
slowness | 0:c03cffe402df | 199 | char data[2]; |
slowness | 0:c03cffe402df | 200 | int ErrorCode = read(_ID, AX12_REG_PRESENT_SPEED, 2, data); |
slowness | 0:c03cffe402df | 201 | int speed = data[0] | (data[1]<<8); |
slowness | 0:c03cffe402df | 202 | return(speed); |
slowness | 0:c03cffe402df | 203 | } |
slowness | 0:c03cffe402df | 204 | |
slowness | 0:c03cffe402df | 205 | |
slowness | 0:c03cffe402df | 206 | int AX12::Get_CCW_Angle_Limit (void) |
slowness | 0:c03cffe402df | 207 | { |
slowness | 0:c03cffe402df | 208 | char data[2]; |
slowness | 0:c03cffe402df | 209 | int ErrorCode = read(_ID, AX12_REG_CCW_LIMIT, 2, data); |
slowness | 0:c03cffe402df | 210 | int angle = data[0] | (data[1]<<8); |
slowness | 0:c03cffe402df | 211 | angle = (angle * 300) / 1023; |
slowness | 0:c03cffe402df | 212 | return(angle); |
slowness | 0:c03cffe402df | 213 | } |
slowness | 0:c03cffe402df | 214 | |
slowness | 0:c03cffe402df | 215 | |
slowness | 0:c03cffe402df | 216 | int AX12::Get_CW_Angle_Limit (void) |
slowness | 0:c03cffe402df | 217 | { |
slowness | 0:c03cffe402df | 218 | char data[2]; |
slowness | 0:c03cffe402df | 219 | int ErrorCode = read(_ID, AX12_REG_CW_LIMIT, 2, data); |
slowness | 0:c03cffe402df | 220 | int angle = data[0] | (data[1]<<8); |
slowness | 0:c03cffe402df | 221 | angle = (angle * 300) / 1023; |
slowness | 0:c03cffe402df | 222 | return(angle); |
slowness | 0:c03cffe402df | 223 | } |
slowness | 0:c03cffe402df | 224 | |
slowness | 0:c03cffe402df | 225 | |
slowness | 0:c03cffe402df | 226 | |
slowness | 0:c03cffe402df | 227 | int AX12::Get_Torque_Enable(void) |
slowness | 0:c03cffe402df | 228 | { |
slowness | 0:c03cffe402df | 229 | char data[1]; |
slowness | 0:c03cffe402df | 230 | int ErrorCode = read(_ID, AX12_REG_TORQUE_ENABLE, 1, data); |
slowness | 0:c03cffe402df | 231 | int enable = data[0]; |
slowness | 0:c03cffe402df | 232 | return(enable); |
slowness | 0:c03cffe402df | 233 | } |
slowness | 0:c03cffe402df | 234 | |
slowness | 0:c03cffe402df | 235 | |
slowness | 0:c03cffe402df | 236 | int AX12::Set_Torque_Enable(int etat) |
slowness | 0:c03cffe402df | 237 | { |
slowness | 0:c03cffe402df | 238 | char data[1]; |
slowness | 0:c03cffe402df | 239 | data [0] = etat; |
slowness | 0:c03cffe402df | 240 | |
slowness | 0:c03cffe402df | 241 | int error = write(_ID, AX12_REG_TORQUE_ENABLE, 1, data); |
slowness | 0:c03cffe402df | 242 | return (error); |
slowness | 0:c03cffe402df | 243 | } |
slowness | 0:c03cffe402df | 244 | |
slowness | 0:c03cffe402df | 245 | |
slowness | 0:c03cffe402df | 246 | |
slowness | 0:c03cffe402df | 247 | int AX12::Get_Up_Calibration (void) |
slowness | 0:c03cffe402df | 248 | { |
slowness | 0:c03cffe402df | 249 | char data[1]; |
slowness | 0:c03cffe402df | 250 | int ErrorCode = read(_ID, AX12_REG_UP_CALIBRATION, 2, data); |
slowness | 0:c03cffe402df | 251 | int Up_calibration = data[0] | (data[1]<<8); |
slowness | 0:c03cffe402df | 252 | return(Up_calibration); |
slowness | 0:c03cffe402df | 253 | } |
slowness | 0:c03cffe402df | 254 | |
slowness | 0:c03cffe402df | 255 | |
slowness | 0:c03cffe402df | 256 | |
slowness | 0:c03cffe402df | 257 | int AX12::Get_Down_Calibration (void) |
slowness | 0:c03cffe402df | 258 | { |
slowness | 0:c03cffe402df | 259 | char data[1]; |
slowness | 0:c03cffe402df | 260 | int ErrorCode = read(_ID, AX12_REG_DOWN_CALIBRATION, 2, data); |
slowness | 0:c03cffe402df | 261 | int Dowm_calibration = data[0] | (data[1]<<8); |
slowness | 0:c03cffe402df | 262 | return(Dowm_calibration); |
slowness | 0:c03cffe402df | 263 | } |
slowness | 0:c03cffe402df | 264 | |
slowness | 0:c03cffe402df | 265 | |
slowness | 0:c03cffe402df | 266 | |
slowness | 0:c03cffe402df | 267 | int AX12::Get_ID(void) |
slowness | 0:c03cffe402df | 268 | { |
slowness | 0:c03cffe402df | 269 | |
slowness | 0:c03cffe402df | 270 | char data[1]={-1}; |
slowness | 0:c03cffe402df | 271 | int ErrorCode = read(_ID, AX12_REG_ID, 1, data); |
slowness | 0:c03cffe402df | 272 | int id = data[0]; |
slowness | 0:c03cffe402df | 273 | return(id); |
slowness | 0:c03cffe402df | 274 | } |
slowness | 0:c03cffe402df | 275 | |
slowness | 0:c03cffe402df | 276 | |
slowness | 0:c03cffe402df | 277 | // Lecture du couple maximum ( retourne la valeur du registre Max Torque de l'AX12 ) |
slowness | 0:c03cffe402df | 278 | int AX12::Get_Max_Torque (void) |
slowness | 0:c03cffe402df | 279 | { |
slowness | 0:c03cffe402df | 280 | char data[2]; |
slowness | 0:c03cffe402df | 281 | int ErrorCode = read(_ID, AX12_REG_MAX_TORQUE, 2, data); |
slowness | 0:c03cffe402df | 282 | int torque = data[0] | (data[1]<<8); |
slowness | 0:c03cffe402df | 283 | return(torque); |
slowness | 0:c03cffe402df | 284 | } |
slowness | 0:c03cffe402df | 285 | |
slowness | 0:c03cffe402df | 286 | |
slowness | 0:c03cffe402df | 287 | |
slowness | 0:c03cffe402df | 288 | /** Reglage du couple maximum de l'actionneur |
slowness | 0:c03cffe402df | 289 | // minimum >> Ox000 >> decimal 0 |
slowness | 0:c03cffe402df | 290 | // maximum >> 0x3FF >> decimal 1023 |
slowness | 0:c03cffe402df | 291 | // deflaut >> >> decimal |
slowness | 0:c03cffe402df | 292 | */ |
slowness | 0:c03cffe402df | 293 | int AX12::Set_Max_Torque(int torque) |
slowness | 0:c03cffe402df | 294 | { |
slowness | 0:c03cffe402df | 295 | char data[2]; |
slowness | 0:c03cffe402df | 296 | |
slowness | 0:c03cffe402df | 297 | data[0] = torque & 0xff; // bottom 8 bits |
slowness | 0:c03cffe402df | 298 | data[1] = torque >> 8; // top 8 bits |
slowness | 0:c03cffe402df | 299 | |
slowness | 0:c03cffe402df | 300 | // write the packet, return the error code |
slowness | 0:c03cffe402df | 301 | return (write(_ID, AX12_REG_MAX_TORQUE, 2, data)); |
slowness | 0:c03cffe402df | 302 | |
slowness | 0:c03cffe402df | 303 | } |
slowness | 0:c03cffe402df | 304 | |
slowness | 0:c03cffe402df | 305 | |
slowness | 0:c03cffe402df | 306 | |
slowness | 0:c03cffe402df | 307 | |
slowness | 0:c03cffe402df | 308 | /** Reglage de la desactivation des actionneurs si une erreur le concernant se produit |
slowness | 0:c03cffe402df | 309 | Bit Function |
slowness | 0:c03cffe402df | 310 | Bit 7 0 |
slowness | 0:c03cffe402df | 311 | Bit 6 If set to 1, torque off when an Instruction Error occurs |
slowness | 0:c03cffe402df | 312 | Bit 5 If set to 1, torque off when an Overload Error occurs |
slowness | 0:c03cffe402df | 313 | Bit 4 If set to 1, torque off when a Checksum Error occurs |
slowness | 0:c03cffe402df | 314 | Bit 3 If set to 1, torque off when a Range Error occurs |
slowness | 0:c03cffe402df | 315 | Bit 2 If set to 1, torque off when an Overheating Error occurs |
slowness | 0:c03cffe402df | 316 | Bit 1 If set to 1, torque off when an Angle Limit Error occurs |
slowness | 0:c03cffe402df | 317 | Bit 0 If set to 1, torque off when an Input Voltage Error occurs |
slowness | 0:c03cffe402df | 318 | */ |
slowness | 0:c03cffe402df | 319 | int AX12::Set_Alarm_Shutdown(int valeur) |
slowness | 0:c03cffe402df | 320 | { |
slowness | 0:c03cffe402df | 321 | char data[1]; |
slowness | 0:c03cffe402df | 322 | data [0] = valeur; |
slowness | 0:c03cffe402df | 323 | |
slowness | 0:c03cffe402df | 324 | int val_alarm_shutdown = write(_ID, AX12_REG_ALARM_SHUTDOWN, 1, data); |
slowness | 0:c03cffe402df | 325 | return (val_alarm_shutdown); |
slowness | 0:c03cffe402df | 326 | } |
slowness | 0:c03cffe402df | 327 | |
slowness | 0:c03cffe402df | 328 | |
slowness | 0:c03cffe402df | 329 | |
slowness | 0:c03cffe402df | 330 | /** Reglage de l'activation de l'alarme |
slowness | 0:c03cffe402df | 331 | Bit Function |
slowness | 0:c03cffe402df | 332 | Bit 7 0 |
slowness | 0:c03cffe402df | 333 | Bit 6 If set to 1, the LED blinks when an Instruction Error occurs |
slowness | 0:c03cffe402df | 334 | Bit 5 If set to 1, the LED blinks when an Overload Error occurs |
slowness | 0:c03cffe402df | 335 | Bit 4 If set to 1, the LED blinks when a Checksum Error occurs |
slowness | 0:c03cffe402df | 336 | Bit 3 If set to 1, the LED blinks when a Range Error occurs |
slowness | 0:c03cffe402df | 337 | Bit 2 If set to 1, the LED blinks when an Overheating Error occurs |
slowness | 0:c03cffe402df | 338 | Bit 1 If set to 1, the LED blinks when an Angle Limit Error occurs |
slowness | 0:c03cffe402df | 339 | Bit 0 If set to 1, the LED blinks when an Input Voltage Error occurs |
slowness | 0:c03cffe402df | 340 | */ |
slowness | 0:c03cffe402df | 341 | int AX12::Set_Alarm_LED(int valeur) |
slowness | 0:c03cffe402df | 342 | { |
slowness | 0:c03cffe402df | 343 | char data[1]; |
slowness | 0:c03cffe402df | 344 | data [0] = valeur; |
slowness | 0:c03cffe402df | 345 | |
slowness | 0:c03cffe402df | 346 | int val_alarmLED = write(_ID, AX12_REG_ALARM_LED, 1, data); |
slowness | 0:c03cffe402df | 347 | return (val_alarmLED); |
slowness | 0:c03cffe402df | 348 | } |
slowness | 0:c03cffe402df | 349 | |
slowness | 0:c03cffe402df | 350 | |
slowness | 0:c03cffe402df | 351 | |
slowness | 0:c03cffe402df | 352 | |
slowness | 0:c03cffe402df | 353 | // Reglage de la réponse à une instruction |
slowness | 0:c03cffe402df | 354 | // 0 >> ne repond a aucune instructions |
slowness | 0:c03cffe402df | 355 | // 1 >> repond seulement aux instructions READ_DATA |
slowness | 0:c03cffe402df | 356 | // 2 >> repond à toutes les instructions |
slowness | 0:c03cffe402df | 357 | int AX12::Set_Status_Return_Level(int etat) |
slowness | 0:c03cffe402df | 358 | { |
slowness | 0:c03cffe402df | 359 | char data[1]; |
slowness | 0:c03cffe402df | 360 | data [0] = etat; |
slowness | 0:c03cffe402df | 361 | |
slowness | 0:c03cffe402df | 362 | int val_return_lvl = write(_ID, AX12_REG_SATUS_RETURN, 1, data); |
slowness | 0:c03cffe402df | 363 | return (val_return_lvl); |
slowness | 0:c03cffe402df | 364 | } |
slowness | 0:c03cffe402df | 365 | |
slowness | 0:c03cffe402df | 366 | |
slowness | 0:c03cffe402df | 367 | // Reglage de la tension minimale |
slowness | 0:c03cffe402df | 368 | // minimum >> Ox32 >> decimal 50 |
slowness | 0:c03cffe402df | 369 | // maximum >> 0xFA >> decimal 250 |
slowness | 0:c03cffe402df | 370 | // deflaut >> 0x3C >> decimal 60 |
slowness | 0:c03cffe402df | 371 | int AX12::Set_Lowest_Voltage(int val_lowest_voltage) |
slowness | 0:c03cffe402df | 372 | { |
slowness | 0:c03cffe402df | 373 | char data[1]; |
slowness | 0:c03cffe402df | 374 | data [0] = val_lowest_voltage; |
slowness | 0:c03cffe402df | 375 | |
slowness | 0:c03cffe402df | 376 | int val_lowvolt = write(_ID, AX12_REG_LOWEST_VOLTAGE, 1, data); |
slowness | 0:c03cffe402df | 377 | return (val_lowvolt); |
slowness | 0:c03cffe402df | 378 | } |
slowness | 0:c03cffe402df | 379 | |
slowness | 0:c03cffe402df | 380 | |
slowness | 0:c03cffe402df | 381 | // Reglage de la tension maximale |
slowness | 0:c03cffe402df | 382 | // minimum >> Ox32 >> decimal 50 |
slowness | 0:c03cffe402df | 383 | // maximum >> 0xFA >> decimal 250 |
slowness | 0:c03cffe402df | 384 | // deflaut >> 0xBE >> decimal 190 |
slowness | 0:c03cffe402df | 385 | int AX12::Set_Highest_Voltage(int val_highest_voltage) |
slowness | 0:c03cffe402df | 386 | { |
slowness | 0:c03cffe402df | 387 | char data[1]; |
slowness | 0:c03cffe402df | 388 | data [0] = val_highest_voltage; |
slowness | 0:c03cffe402df | 389 | |
slowness | 0:c03cffe402df | 390 | int val_highvolt = write(_ID, AX12_REG_HIGHEST_VOLTAGE, 1, data); |
slowness | 0:c03cffe402df | 391 | return (val_highvolt); |
slowness | 0:c03cffe402df | 392 | } |
slowness | 0:c03cffe402df | 393 | |
slowness | 0:c03cffe402df | 394 | |
slowness | 0:c03cffe402df | 395 | // Reglage du return time delay EN MICRO SECONDE 2uSec * val_delay_time |
slowness | 0:c03cffe402df | 396 | // minimum >> 0 us |
slowness | 0:c03cffe402df | 397 | // maximum >> 508 us |
slowness | 0:c03cffe402df | 398 | // deflaut >> 125 us |
slowness | 0:c03cffe402df | 399 | int AX12::Set_Delay_Time (int val_delay_time ) |
slowness | 0:c03cffe402df | 400 | { |
slowness | 0:c03cffe402df | 401 | char data[1]; |
slowness | 0:c03cffe402df | 402 | data [0] = val_delay_time/2.0; |
slowness | 0:c03cffe402df | 403 | |
slowness | 0:c03cffe402df | 404 | int valdelay_time = write(_ID, AX12_REG_DELAY_TIME, 1, data); |
slowness | 0:c03cffe402df | 405 | return (valdelay_time ); |
slowness | 0:c03cffe402df | 406 | } |
slowness | 0:c03cffe402df | 407 | |
slowness | 0:c03cffe402df | 408 | |
slowness | 0:c03cffe402df | 409 | // Reglage de la température max du cervo |
slowness | 0:c03cffe402df | 410 | // minimum >> Ox00 >> decimal 0 |
slowness | 0:c03cffe402df | 411 | // maximum >> 0x96 >> decimal 150 |
slowness | 0:c03cffe402df | 412 | int AX12::Set_Temperature_Max (int val_temperature ) |
slowness | 0:c03cffe402df | 413 | { |
slowness | 0:c03cffe402df | 414 | char data[1]; |
slowness | 0:c03cffe402df | 415 | data [0] = val_temperature; |
slowness | 0:c03cffe402df | 416 | |
slowness | 0:c03cffe402df | 417 | int valtemp_max = write(_ID, AX12_REG_TEMP_MAX, 1, data); |
slowness | 0:c03cffe402df | 418 | return (valtemp_max ); |
slowness | 0:c03cffe402df | 419 | } |
slowness | 0:c03cffe402df | 420 | |
slowness | 0:c03cffe402df | 421 | // Etat LED |
slowness | 0:c03cffe402df | 422 | // 0 = off |
slowness | 0:c03cffe402df | 423 | // 1 = on |
slowness | 0:c03cffe402df | 424 | int AX12::Set_Etat_LED(int etat) |
slowness | 0:c03cffe402df | 425 | { |
slowness | 0:c03cffe402df | 426 | char data[1]; |
slowness | 0:c03cffe402df | 427 | data [0] = etat; |
slowness | 0:c03cffe402df | 428 | |
slowness | 0:c03cffe402df | 429 | int valLED = write(_ID, AX12_REG_LED, 1, data); |
slowness | 0:c03cffe402df | 430 | return (valLED); |
slowness | 0:c03cffe402df | 431 | } |
slowness | 0:c03cffe402df | 432 | |
slowness | 0:c03cffe402df | 433 | // Set the mode of the servo |
slowness | 0:c03cffe402df | 434 | // 0 = Positional (0-300 degrees) |
slowness | 0:c03cffe402df | 435 | // 1 = Rotational -1 to 1 speed |
slowness | 0:c03cffe402df | 436 | int AX12::Set_Mode(int mode) |
slowness | 0:c03cffe402df | 437 | { |
slowness | 0:c03cffe402df | 438 | |
slowness | 0:c03cffe402df | 439 | if (mode == 1) { // set CR |
slowness | 0:c03cffe402df | 440 | //wait(0.001); |
slowness | 0:c03cffe402df | 441 | Set_CW_Angle_Limit(0); |
slowness | 0:c03cffe402df | 442 | //wait(0.001); |
slowness | 0:c03cffe402df | 443 | Set_CCW_Angle_Limit(0); |
slowness | 0:c03cffe402df | 444 | //wait(0.001); |
slowness | 0:c03cffe402df | 445 | Set_CR_Speed(0.0); |
slowness | 0:c03cffe402df | 446 | //wait(0.001); |
slowness | 0:c03cffe402df | 447 | } else { |
slowness | 0:c03cffe402df | 448 | //wait(0.001); |
slowness | 0:c03cffe402df | 449 | Set_CW_Angle_Limit(0); |
slowness | 0:c03cffe402df | 450 | //wait(0.001); |
slowness | 0:c03cffe402df | 451 | Set_CCW_Angle_Limit(300); |
slowness | 0:c03cffe402df | 452 | //wait(0.001); |
slowness | 0:c03cffe402df | 453 | Set_CR_Speed(0.0); |
slowness | 0:c03cffe402df | 454 | //wait(0.001); |
slowness | 0:c03cffe402df | 455 | } |
slowness | 0:c03cffe402df | 456 | return(0); |
slowness | 0:c03cffe402df | 457 | } |
slowness | 0:c03cffe402df | 458 | |
slowness | 1:fce3323d874f | 459 | int AX12::Set_Goal_speed(int speed, int flags) |
slowness | 1:fce3323d874f | 460 | { |
slowness | 1:fce3323d874f | 461 | |
slowness | 1:fce3323d874f | 462 | char reg_flag = 0; |
slowness | 1:fce3323d874f | 463 | char data[2]; |
slowness | 1:fce3323d874f | 464 | |
slowness | 1:fce3323d874f | 465 | // set the flag is only the register bit is set in the flag |
slowness | 1:fce3323d874f | 466 | if (flags == 0x2) { |
slowness | 1:fce3323d874f | 467 | reg_flag = 1; |
slowness | 1:fce3323d874f | 468 | } |
slowness | 1:fce3323d874f | 469 | |
slowness | 1:fce3323d874f | 470 | data[0] = speed & 0xff; // bottom 8 bits |
slowness | 1:fce3323d874f | 471 | data[1] = speed >> 8; // top 8 bits |
slowness | 1:fce3323d874f | 472 | |
slowness | 1:fce3323d874f | 473 | // write the packet, return the error code |
slowness | 1:fce3323d874f | 474 | int rVal = write(_ID, AX12_REG_MOVING_SPEED, 2, data, reg_flag); |
slowness | 1:fce3323d874f | 475 | |
slowness | 1:fce3323d874f | 476 | /*if (flags == 1) { |
slowness | 1:fce3323d874f | 477 | // block until it comes to a halt |
slowness | 1:fce3323d874f | 478 | while (isMoving()) |
slowness | 1:fce3323d874f | 479 | { |
slowness | 1:fce3323d874f | 480 | } |
slowness | 1:fce3323d874f | 481 | |
slowness | 1:fce3323d874f | 482 | }*/ |
slowness | 1:fce3323d874f | 483 | return(rVal); |
slowness | 1:fce3323d874f | 484 | } |
slowness | 1:fce3323d874f | 485 | |
slowness | 0:c03cffe402df | 486 | |
slowness | 0:c03cffe402df | 487 | // if flag[0] is set, were blocking |
slowness | 0:c03cffe402df | 488 | // if flag[1] is set, we're registering |
slowness | 0:c03cffe402df | 489 | // they are mutually exclusive operations |
slowness | 0:c03cffe402df | 490 | int AX12::Set_Goal(int degrees, int flags) |
slowness | 0:c03cffe402df | 491 | { |
slowness | 0:c03cffe402df | 492 | |
slowness | 0:c03cffe402df | 493 | char reg_flag = 0; |
slowness | 0:c03cffe402df | 494 | char data[2]; |
slowness | 0:c03cffe402df | 495 | |
slowness | 0:c03cffe402df | 496 | // set the flag is only the register bit is set in the flag |
slowness | 0:c03cffe402df | 497 | if (flags == 0x2) { |
slowness | 0:c03cffe402df | 498 | reg_flag = 1; |
slowness | 0:c03cffe402df | 499 | } |
slowness | 0:c03cffe402df | 500 | |
slowness | 0:c03cffe402df | 501 | // 1023 / 300 * degrees |
slowness | 0:c03cffe402df | 502 | short goal = (1023 * degrees) / 300; |
slowness | 0:c03cffe402df | 503 | #ifdef AX12_DEBUG |
slowness | 0:c03cffe402df | 504 | printf("SetGoal to 0x%x\n",goal); |
slowness | 0:c03cffe402df | 505 | #endif |
slowness | 0:c03cffe402df | 506 | |
slowness | 0:c03cffe402df | 507 | data[0] = goal & 0xff; // bottom 8 bits |
slowness | 0:c03cffe402df | 508 | data[1] = goal >> 8; // top 8 bits |
slowness | 0:c03cffe402df | 509 | |
slowness | 0:c03cffe402df | 510 | // write the packet, return the error code |
slowness | 0:c03cffe402df | 511 | int rVal = write(_ID, AX12_REG_GOAL_POSITION, 2, data, reg_flag); |
slowness | 0:c03cffe402df | 512 | |
slowness | 0:c03cffe402df | 513 | /*if (flags == 1) { |
slowness | 0:c03cffe402df | 514 | // block until it comes to a halt |
slowness | 0:c03cffe402df | 515 | while (isMoving()) |
slowness | 0:c03cffe402df | 516 | { |
slowness | 0:c03cffe402df | 517 | } |
slowness | 0:c03cffe402df | 518 | |
slowness | 0:c03cffe402df | 519 | }*/ |
slowness | 0:c03cffe402df | 520 | return(rVal); |
slowness | 0:c03cffe402df | 521 | } |
slowness | 0:c03cffe402df | 522 | |
slowness | 0:c03cffe402df | 523 | |
slowness | 0:c03cffe402df | 524 | // Set continuous rotation speed from -1 to 1 |
slowness | 0:c03cffe402df | 525 | int AX12::Set_CR_Speed(float speed) |
slowness | 0:c03cffe402df | 526 | { |
slowness | 0:c03cffe402df | 527 | |
slowness | 0:c03cffe402df | 528 | // bit 10 = direction, 0 = CCW, 1=CW |
slowness | 0:c03cffe402df | 529 | // bits 9-0 = Speed |
slowness | 0:c03cffe402df | 530 | char data[2]; |
slowness | 0:c03cffe402df | 531 | |
slowness | 0:c03cffe402df | 532 | int goal = (0x3ff * abs(speed)); |
slowness | 0:c03cffe402df | 533 | |
slowness | 0:c03cffe402df | 534 | // Set direction CW if we have a negative speed |
slowness | 0:c03cffe402df | 535 | if (speed < 0) { |
slowness | 0:c03cffe402df | 536 | goal |= (0x1 << 10); |
slowness | 0:c03cffe402df | 537 | } |
slowness | 0:c03cffe402df | 538 | |
slowness | 0:c03cffe402df | 539 | data[0] = goal & 0xff; // bottom 8 bits |
slowness | 0:c03cffe402df | 540 | data[1] = goal >> 8; // top 8 bits |
slowness | 0:c03cffe402df | 541 | |
slowness | 0:c03cffe402df | 542 | // write the packet, return the error code |
slowness | 0:c03cffe402df | 543 | int rVal = write(_ID, 0x20, 2, data); |
slowness | 0:c03cffe402df | 544 | |
slowness | 0:c03cffe402df | 545 | return(rVal); |
slowness | 0:c03cffe402df | 546 | } |
slowness | 0:c03cffe402df | 547 | |
slowness | 0:c03cffe402df | 548 | |
slowness | 0:c03cffe402df | 549 | int AX12::Set_CW_Angle_Limit (int degrees) |
slowness | 0:c03cffe402df | 550 | { |
slowness | 0:c03cffe402df | 551 | |
slowness | 0:c03cffe402df | 552 | char data[2]; |
slowness | 0:c03cffe402df | 553 | |
slowness | 0:c03cffe402df | 554 | // 1023 / 300 * degrees |
slowness | 0:c03cffe402df | 555 | short limit = (1023 * degrees) / 300; |
slowness | 0:c03cffe402df | 556 | |
slowness | 0:c03cffe402df | 557 | #ifdef AX12_DEBUG |
slowness | 0:c03cffe402df | 558 | printf("SetCWLimit to 0x%x\n",limit); |
slowness | 0:c03cffe402df | 559 | #endif |
slowness | 0:c03cffe402df | 560 | |
slowness | 0:c03cffe402df | 561 | data[0] = limit & 0xff; // bottom 8 bits |
slowness | 0:c03cffe402df | 562 | data[1] = limit >> 8; // top 8 bits |
slowness | 0:c03cffe402df | 563 | |
slowness | 0:c03cffe402df | 564 | // write the packet, return the error code |
slowness | 0:c03cffe402df | 565 | return (write(_ID, AX12_REG_CW_LIMIT, 2, data)); |
slowness | 0:c03cffe402df | 566 | |
slowness | 0:c03cffe402df | 567 | } |
slowness | 0:c03cffe402df | 568 | |
slowness | 0:c03cffe402df | 569 | int AX12::Set_CCW_Angle_Limit (int degrees) |
slowness | 0:c03cffe402df | 570 | { |
slowness | 0:c03cffe402df | 571 | |
slowness | 0:c03cffe402df | 572 | char data[2]; |
slowness | 0:c03cffe402df | 573 | |
slowness | 0:c03cffe402df | 574 | // 1023 / 300 * degrees |
slowness | 0:c03cffe402df | 575 | short limit = (1023 * degrees) / 300; |
slowness | 0:c03cffe402df | 576 | |
slowness | 0:c03cffe402df | 577 | #ifdef AX12_DEBUG |
slowness | 0:c03cffe402df | 578 | printf("SetCCWLimit to 0x%x\n",limit); |
slowness | 0:c03cffe402df | 579 | #endif |
slowness | 0:c03cffe402df | 580 | |
slowness | 0:c03cffe402df | 581 | data[0] = limit & 0xff; // bottom 8 bits |
slowness | 0:c03cffe402df | 582 | data[1] = limit >> 8; // top 8 bits |
slowness | 0:c03cffe402df | 583 | |
slowness | 0:c03cffe402df | 584 | // write the packet, return the error code |
slowness | 0:c03cffe402df | 585 | return (write(_ID, AX12_REG_CCW_LIMIT, 2, data)); |
slowness | 0:c03cffe402df | 586 | } |
slowness | 0:c03cffe402df | 587 | |
slowness | 0:c03cffe402df | 588 | |
slowness | 0:c03cffe402df | 589 | int AX12::Set_ID (int CurrentID, int NewID) |
slowness | 0:c03cffe402df | 590 | { |
slowness | 0:c03cffe402df | 591 | |
slowness | 0:c03cffe402df | 592 | char data[1]; |
slowness | 0:c03cffe402df | 593 | data[0] = NewID; |
slowness | 0:c03cffe402df | 594 | |
slowness | 0:c03cffe402df | 595 | #ifdef AX12_DEBUG |
slowness | 0:c03cffe402df | 596 | printf("Setting ID from 0x%x to 0x%x\n",CurrentID,NewID); |
slowness | 0:c03cffe402df | 597 | #endif |
slowness | 0:c03cffe402df | 598 | |
slowness | 0:c03cffe402df | 599 | return (write(CurrentID, AX12_REG_ID, 1, data)); |
slowness | 0:c03cffe402df | 600 | |
slowness | 0:c03cffe402df | 601 | } |
slowness | 0:c03cffe402df | 602 | |
slowness | 0:c03cffe402df | 603 | |
slowness | 0:c03cffe402df | 604 | int AX12::Set_Baud (int baud) |
slowness | 0:c03cffe402df | 605 | { |
slowness | 0:c03cffe402df | 606 | |
slowness | 0:c03cffe402df | 607 | char data[1]; |
slowness | 0:c03cffe402df | 608 | data[0] = baud; |
slowness | 0:c03cffe402df | 609 | |
slowness | 0:c03cffe402df | 610 | #ifdef AX12_DEBUG |
slowness | 0:c03cffe402df | 611 | printf("Setting Baud rate to %d\n",baud); |
slowness | 0:c03cffe402df | 612 | #endif |
slowness | 0:c03cffe402df | 613 | |
slowness | 0:c03cffe402df | 614 | return (write(_ID, AX12_REG_BAUD, 1, data)); |
slowness | 0:c03cffe402df | 615 | |
slowness | 0:c03cffe402df | 616 | } |
slowness | 0:c03cffe402df | 617 | |
slowness | 0:c03cffe402df | 618 | |
slowness | 0:c03cffe402df | 619 | |
slowness | 0:c03cffe402df | 620 | // return 1 is the servo is still in flight |
slowness | 0:c03cffe402df | 621 | int AX12::isMoving(void) |
slowness | 0:c03cffe402df | 622 | { |
slowness | 0:c03cffe402df | 623 | |
slowness | 0:c03cffe402df | 624 | char data[1]; |
slowness | 0:c03cffe402df | 625 | read(_ID,AX12_REG_MOVING,1,data); |
slowness | 0:c03cffe402df | 626 | return(data[0]); |
slowness | 0:c03cffe402df | 627 | } |
slowness | 0:c03cffe402df | 628 | |
slowness | 0:c03cffe402df | 629 | void AX12::reset() |
slowness | 0:c03cffe402df | 630 | { |
slowness | 0:c03cffe402df | 631 | |
slowness | 0:c03cffe402df | 632 | unsigned char TxBuf[16]; |
slowness | 0:c03cffe402df | 633 | unsigned char sum = 0; |
slowness | 0:c03cffe402df | 634 | unsigned long debut=0; |
slowness | 0:c03cffe402df | 635 | |
slowness | 0:c03cffe402df | 636 | #ifdef AX12_TRIGGER_DEBUG |
slowness | 0:c03cffe402df | 637 | // Build the TxPacket first in RAM, then we'll send in one go |
slowness | 0:c03cffe402df | 638 | printf("\nreset\n"); |
slowness | 0:c03cffe402df | 639 | printf("\nreset Packet\n Header : 0xFF, 0xFF\n"); |
slowness | 0:c03cffe402df | 640 | #endif |
slowness | 0:c03cffe402df | 641 | |
slowness | 0:c03cffe402df | 642 | TxBuf[0] = 0xFF; |
slowness | 0:c03cffe402df | 643 | TxBuf[1] = 0xFF; |
slowness | 0:c03cffe402df | 644 | |
slowness | 0:c03cffe402df | 645 | // ID - Broadcast |
slowness | 0:c03cffe402df | 646 | TxBuf[2] =_ID; |
slowness | 0:c03cffe402df | 647 | sum += TxBuf[2]; |
slowness | 0:c03cffe402df | 648 | |
slowness | 0:c03cffe402df | 649 | #ifdef AX12_TRIGGER_DEBUG |
slowness | 0:c03cffe402df | 650 | printf(" ID : %d\n",TxBuf[2]); |
slowness | 0:c03cffe402df | 651 | #endif |
slowness | 0:c03cffe402df | 652 | |
slowness | 0:c03cffe402df | 653 | // Length |
slowness | 0:c03cffe402df | 654 | TxBuf[3] = 0x02; |
slowness | 0:c03cffe402df | 655 | sum += TxBuf[3]; |
slowness | 0:c03cffe402df | 656 | |
slowness | 0:c03cffe402df | 657 | #ifdef AX12_TRIGGER_DEBUG |
slowness | 0:c03cffe402df | 658 | printf(" Length %d\n",TxBuf[3]); |
slowness | 0:c03cffe402df | 659 | #endif |
slowness | 0:c03cffe402df | 660 | |
slowness | 0:c03cffe402df | 661 | // Instruction - ACTION |
slowness | 0:c03cffe402df | 662 | TxBuf[4] = 0x06; //reset |
slowness | 0:c03cffe402df | 663 | sum += TxBuf[4]; |
slowness | 0:c03cffe402df | 664 | |
slowness | 0:c03cffe402df | 665 | #ifdef AX12_TRIGGER_DEBUG |
slowness | 0:c03cffe402df | 666 | printf(" Instruction 0x%X\n",TxBuf[5]); |
slowness | 0:c03cffe402df | 667 | #endif |
slowness | 0:c03cffe402df | 668 | |
slowness | 0:c03cffe402df | 669 | // Checksum |
slowness | 0:c03cffe402df | 670 | TxBuf[5] = 0xFF - sum; |
slowness | 0:c03cffe402df | 671 | //#ifdef AX12_TRIGGER_DEBUG |
slowness | 0:c03cffe402df | 672 | printf(" Checksum 0x%X\n",TxBuf[5]); |
slowness | 0:c03cffe402df | 673 | //#endif |
slowness | 0:c03cffe402df | 674 | |
slowness | 0:c03cffe402df | 675 | // Transmit the packet in one burst with no pausing |
slowness | 0:c03cffe402df | 676 | for (int i = 0; i < 6 ; i++) |
slowness | 0:c03cffe402df | 677 | { |
slowness | 0:c03cffe402df | 678 | while(_ax12.writeable()==0); |
slowness | 0:c03cffe402df | 679 | _ax12.putc(TxBuf[i]); |
slowness | 0:c03cffe402df | 680 | |
slowness | 0:c03cffe402df | 681 | } |
slowness | 0:c03cffe402df | 682 | wait(0.001); |
slowness | 0:c03cffe402df | 683 | debut=t.read_ms(); |
slowness | 0:c03cffe402df | 684 | |
slowness | 0:c03cffe402df | 685 | do |
slowness | 0:c03cffe402df | 686 | { |
slowness | 0:c03cffe402df | 687 | if (_ax12.readable()==-1) // reception du premier Header ( 0xFF ) |
slowness | 0:c03cffe402df | 688 | printf("%02x",_ax12.getc()); |
slowness | 0:c03cffe402df | 689 | } |
slowness | 0:c03cffe402df | 690 | while((t.read_ms()-debut)<500); |
slowness | 0:c03cffe402df | 691 | |
slowness | 0:c03cffe402df | 692 | printf("\n"); |
slowness | 0:c03cffe402df | 693 | return; |
slowness | 0:c03cffe402df | 694 | } |
slowness | 0:c03cffe402df | 695 | |
slowness | 0:c03cffe402df | 696 | void AX12::read_all_info(unsigned char start, unsigned char longueur) |
slowness | 0:c03cffe402df | 697 | { |
slowness | 0:c03cffe402df | 698 | |
slowness | 0:c03cffe402df | 699 | unsigned char TxBuf[16]; |
slowness | 0:c03cffe402df | 700 | unsigned char sum = 0; |
slowness | 0:c03cffe402df | 701 | unsigned long debut=0; |
slowness | 0:c03cffe402df | 702 | |
slowness | 0:c03cffe402df | 703 | #ifdef AX12_TRIGGER_DEBUG |
slowness | 0:c03cffe402df | 704 | // Build the TxPacket first in RAM, then we'll send in one go |
slowness | 0:c03cffe402df | 705 | printf("\nreset\n"); |
slowness | 0:c03cffe402df | 706 | printf("\nreset Packet\n Header : 0xFF, 0xFF\n"); |
slowness | 0:c03cffe402df | 707 | #endif |
slowness | 0:c03cffe402df | 708 | |
slowness | 0:c03cffe402df | 709 | TxBuf[0] = 0xFF; |
slowness | 0:c03cffe402df | 710 | TxBuf[1] = 0xFF; |
slowness | 0:c03cffe402df | 711 | |
slowness | 0:c03cffe402df | 712 | // ID - Broadcast |
slowness | 0:c03cffe402df | 713 | TxBuf[2] =_ID; |
slowness | 0:c03cffe402df | 714 | sum += TxBuf[2]; |
slowness | 0:c03cffe402df | 715 | |
slowness | 0:c03cffe402df | 716 | #ifdef AX12_TRIGGER_DEBUG |
slowness | 0:c03cffe402df | 717 | printf(" ID : %d\n",TxBuf[2]); |
slowness | 0:c03cffe402df | 718 | #endif |
slowness | 0:c03cffe402df | 719 | |
slowness | 0:c03cffe402df | 720 | // Length |
slowness | 0:c03cffe402df | 721 | TxBuf[3] = 0x04; |
slowness | 0:c03cffe402df | 722 | sum += TxBuf[3]; |
slowness | 0:c03cffe402df | 723 | |
slowness | 0:c03cffe402df | 724 | #ifdef AX12_TRIGGER_DEBUG |
slowness | 0:c03cffe402df | 725 | printf(" Length %d\n",TxBuf[3]); |
slowness | 0:c03cffe402df | 726 | #endif |
slowness | 0:c03cffe402df | 727 | |
slowness | 0:c03cffe402df | 728 | // Instruction - ACTION |
slowness | 0:c03cffe402df | 729 | TxBuf[4] = INST_READ; //reset |
slowness | 0:c03cffe402df | 730 | sum += TxBuf[4]; |
slowness | 0:c03cffe402df | 731 | |
slowness | 0:c03cffe402df | 732 | #ifdef AX12_TRIGGER_DEBUG |
slowness | 0:c03cffe402df | 733 | printf(" Instruction 0x%X\n",TxBuf[5]); |
slowness | 0:c03cffe402df | 734 | #endif |
slowness | 0:c03cffe402df | 735 | |
slowness | 0:c03cffe402df | 736 | TxBuf[5] = start; //reset |
slowness | 0:c03cffe402df | 737 | sum += TxBuf[5]; |
slowness | 0:c03cffe402df | 738 | |
slowness | 0:c03cffe402df | 739 | TxBuf[6] = longueur; //reset |
slowness | 0:c03cffe402df | 740 | sum += TxBuf[6]; |
slowness | 0:c03cffe402df | 741 | |
slowness | 0:c03cffe402df | 742 | |
slowness | 0:c03cffe402df | 743 | // Checksum |
slowness | 0:c03cffe402df | 744 | TxBuf[7] = 0xFF - sum; |
slowness | 0:c03cffe402df | 745 | //#ifdef AX12_TRIGGER_DEBUG |
slowness | 0:c03cffe402df | 746 | //printf(" Checksum 0x%X\n\r",TxBuf[7]); |
slowness | 0:c03cffe402df | 747 | //#endif |
slowness | 0:c03cffe402df | 748 | |
slowness | 0:c03cffe402df | 749 | // Transmit the packet in one burst with no pausing |
slowness | 0:c03cffe402df | 750 | for (int i = 0; i < 8 ; i++) |
slowness | 0:c03cffe402df | 751 | { |
slowness | 0:c03cffe402df | 752 | while(_ax12.writeable()==0); |
slowness | 0:c03cffe402df | 753 | _ax12.putc(TxBuf[i]); |
slowness | 0:c03cffe402df | 754 | |
slowness | 0:c03cffe402df | 755 | } |
slowness | 0:c03cffe402df | 756 | |
slowness | 0:c03cffe402df | 757 | debut=t.read_ms(); |
slowness | 0:c03cffe402df | 758 | int i=0; |
slowness | 0:c03cffe402df | 759 | do |
slowness | 0:c03cffe402df | 760 | { |
slowness | 0:c03cffe402df | 761 | if (_ax12.readable()) |
slowness | 0:c03cffe402df | 762 | { // reception du premier Header ( 0xFF ) |
slowness | 0:c03cffe402df | 763 | printf("%02d:%02x ",start+i,_ax12.getc()); |
slowness | 0:c03cffe402df | 764 | i++; |
slowness | 0:c03cffe402df | 765 | } |
slowness | 0:c03cffe402df | 766 | } |
slowness | 0:c03cffe402df | 767 | while((t.read_ms()-debut)<5000); |
slowness | 0:c03cffe402df | 768 | |
slowness | 0:c03cffe402df | 769 | printf("\n"); |
slowness | 0:c03cffe402df | 770 | return; |
slowness | 0:c03cffe402df | 771 | } |
slowness | 0:c03cffe402df | 772 | |
slowness | 0:c03cffe402df | 773 | |
slowness | 0:c03cffe402df | 774 | void AX12::trigger(void) |
slowness | 0:c03cffe402df | 775 | { |
slowness | 0:c03cffe402df | 776 | |
slowness | 0:c03cffe402df | 777 | char TxBuf[16]; |
slowness | 0:c03cffe402df | 778 | char sum = 0; |
slowness | 0:c03cffe402df | 779 | |
slowness | 0:c03cffe402df | 780 | #ifdef AX12_TRIGGER_DEBUG |
slowness | 0:c03cffe402df | 781 | // Build the TxPacket first in RAM, then we'll send in one go |
slowness | 0:c03cffe402df | 782 | printf("\nTriggered\n"); |
slowness | 0:c03cffe402df | 783 | printf("\nTrigger Packet\n Header : 0xFF, 0xFF\n"); |
slowness | 0:c03cffe402df | 784 | #endif |
slowness | 0:c03cffe402df | 785 | |
slowness | 0:c03cffe402df | 786 | TxBuf[0] = 0xFF; |
slowness | 0:c03cffe402df | 787 | TxBuf[1] = 0xFF; |
slowness | 0:c03cffe402df | 788 | |
slowness | 0:c03cffe402df | 789 | // ID - Broadcast |
slowness | 0:c03cffe402df | 790 | TxBuf[2] = 0xFE; |
slowness | 0:c03cffe402df | 791 | sum += TxBuf[2]; |
slowness | 0:c03cffe402df | 792 | |
slowness | 0:c03cffe402df | 793 | #ifdef AX12_TRIGGER_DEBUG |
slowness | 0:c03cffe402df | 794 | printf(" ID : %d\n",TxBuf[2]); |
slowness | 0:c03cffe402df | 795 | #endif |
slowness | 0:c03cffe402df | 796 | |
slowness | 0:c03cffe402df | 797 | // Length |
slowness | 0:c03cffe402df | 798 | TxBuf[3] = 0x02; |
slowness | 0:c03cffe402df | 799 | sum += TxBuf[3]; |
slowness | 0:c03cffe402df | 800 | |
slowness | 0:c03cffe402df | 801 | #ifdef AX12_TRIGGER_DEBUG |
slowness | 0:c03cffe402df | 802 | printf(" Length %d\n",TxBuf[3]); |
slowness | 0:c03cffe402df | 803 | #endif |
slowness | 0:c03cffe402df | 804 | |
slowness | 0:c03cffe402df | 805 | // Instruction - ACTION |
slowness | 0:c03cffe402df | 806 | TxBuf[4] = 0x04; |
slowness | 0:c03cffe402df | 807 | sum += TxBuf[4]; |
slowness | 0:c03cffe402df | 808 | |
slowness | 0:c03cffe402df | 809 | #ifdef AX12_TRIGGER_DEBUG |
slowness | 0:c03cffe402df | 810 | printf(" Instruction 0x%X\n",TxBuf[5]); |
slowness | 0:c03cffe402df | 811 | #endif |
slowness | 0:c03cffe402df | 812 | |
slowness | 0:c03cffe402df | 813 | // Checksum |
slowness | 0:c03cffe402df | 814 | TxBuf[5] = 0xFF - sum; |
slowness | 0:c03cffe402df | 815 | #ifdef AX12_TRIGGER_DEBUG |
slowness | 0:c03cffe402df | 816 | printf(" Checksum 0x%X\n",TxBuf[5]); |
slowness | 0:c03cffe402df | 817 | #endif |
slowness | 0:c03cffe402df | 818 | |
slowness | 0:c03cffe402df | 819 | // Transmit the packet in one burst with no pausing |
slowness | 0:c03cffe402df | 820 | for (int i = 0; i < 6 ; i++) { |
slowness | 0:c03cffe402df | 821 | _ax12.putc(TxBuf[i]); |
slowness | 0:c03cffe402df | 822 | } |
slowness | 0:c03cffe402df | 823 | |
slowness | 0:c03cffe402df | 824 | // This is a broadcast packet, so there will be no reply |
slowness | 0:c03cffe402df | 825 | return; |
slowness | 0:c03cffe402df | 826 | } |
slowness | 0:c03cffe402df | 827 | |
slowness | 0:c03cffe402df | 828 | |
slowness | 0:c03cffe402df | 829 | float AX12::Get_Position(void) |
slowness | 0:c03cffe402df | 830 | { |
slowness | 0:c03cffe402df | 831 | |
slowness | 0:c03cffe402df | 832 | #ifdef AX12_DEBUG |
slowness | 0:c03cffe402df | 833 | printf("\nGetPosition(%d)",_ID); |
slowness | 0:c03cffe402df | 834 | #endif |
slowness | 0:c03cffe402df | 835 | |
slowness | 0:c03cffe402df | 836 | char data[2]; |
slowness | 0:c03cffe402df | 837 | |
slowness | 0:c03cffe402df | 838 | int ErrorCode = read(_ID, AX12_REG_POSITION, 2, data); |
slowness | 0:c03cffe402df | 839 | int position = data[0] | (data[1] << 8); |
slowness | 0:c03cffe402df | 840 | float angle = ((float)position * 300.0)/1023.0; |
slowness | 0:c03cffe402df | 841 | |
slowness | 0:c03cffe402df | 842 | return (angle); |
slowness | 0:c03cffe402df | 843 | } |
slowness | 0:c03cffe402df | 844 | |
slowness | 0:c03cffe402df | 845 | |
slowness | 0:c03cffe402df | 846 | float AX12::Get_Temp (void) |
slowness | 0:c03cffe402df | 847 | { |
slowness | 0:c03cffe402df | 848 | |
slowness | 0:c03cffe402df | 849 | #ifdef AX12_DEBUG |
slowness | 0:c03cffe402df | 850 | printf("\nGetTemp(%d)",_ID); |
slowness | 0:c03cffe402df | 851 | #endif |
slowness | 0:c03cffe402df | 852 | |
slowness | 0:c03cffe402df | 853 | char data[1]; |
slowness | 0:c03cffe402df | 854 | int ErrorCode = read(_ID, AX12_REG_TEMP, 1, data); |
slowness | 0:c03cffe402df | 855 | float temp = data[0]; |
slowness | 0:c03cffe402df | 856 | return(temp); |
slowness | 0:c03cffe402df | 857 | } |
slowness | 0:c03cffe402df | 858 | |
slowness | 0:c03cffe402df | 859 | |
slowness | 0:c03cffe402df | 860 | float AX12::Get_Volts (void) |
slowness | 0:c03cffe402df | 861 | { |
slowness | 0:c03cffe402df | 862 | |
slowness | 0:c03cffe402df | 863 | #ifdef AX12_DEBUG |
slowness | 0:c03cffe402df | 864 | printf("\nGetVolts(%d)",_ID); |
slowness | 0:c03cffe402df | 865 | #endif |
slowness | 0:c03cffe402df | 866 | |
slowness | 0:c03cffe402df | 867 | char data[1]; |
slowness | 0:c03cffe402df | 868 | int ErrorCode = read(_ID, AX12_REG_VOLTS, 1, data); |
slowness | 0:c03cffe402df | 869 | float volts = data[0]/10.0; |
slowness | 0:c03cffe402df | 870 | return(volts); |
slowness | 0:c03cffe402df | 871 | } |
slowness | 0:c03cffe402df | 872 | |
slowness | 0:c03cffe402df | 873 | |
slowness | 0:c03cffe402df | 874 | int AX12::read(int ID, int start, int bytes, char* data) { |
slowness | 0:c03cffe402df | 875 | |
slowness | 0:c03cffe402df | 876 | |
slowness | 0:c03cffe402df | 877 | char PacketLength = 0x3; |
slowness | 0:c03cffe402df | 878 | char TxBuf[16]; |
slowness | 0:c03cffe402df | 879 | char sum = 0; |
slowness | 0:c03cffe402df | 880 | char Status[16]; |
slowness | 0:c03cffe402df | 881 | |
slowness | 0:c03cffe402df | 882 | int timeout = 0; |
slowness | 0:c03cffe402df | 883 | int plen = 0; |
slowness | 0:c03cffe402df | 884 | int flag_out = 0; |
slowness | 0:c03cffe402df | 885 | int timeout_transmit = 0; |
slowness | 0:c03cffe402df | 886 | int i = 0; |
slowness | 0:c03cffe402df | 887 | int enable = 0; |
slowness | 0:c03cffe402df | 888 | // int poubelle = 0; |
slowness | 0:c03cffe402df | 889 | // int count = 0; |
slowness | 0:c03cffe402df | 890 | // char vidage[50]; |
slowness | 0:c03cffe402df | 891 | |
slowness | 0:c03cffe402df | 892 | typedef enum {Header1, Header2, ident, length, erreur, reception, checksum} type_etat; |
slowness | 0:c03cffe402df | 893 | type_etat etat = Header1; |
slowness | 0:c03cffe402df | 894 | |
slowness | 0:c03cffe402df | 895 | Status[4] = 0xFE; // return code |
slowness | 0:c03cffe402df | 896 | |
slowness | 0:c03cffe402df | 897 | |
slowness | 0:c03cffe402df | 898 | |
slowness | 0:c03cffe402df | 899 | |
slowness | 0:c03cffe402df | 900 | |
slowness | 0:c03cffe402df | 901 | /*********************************** CREATION DE LA TRAME A EVOYER *****************************************/ |
slowness | 0:c03cffe402df | 902 | |
slowness | 0:c03cffe402df | 903 | #ifdef AX12_READ_DEBUG |
slowness | 0:c03cffe402df | 904 | printf("\nread(%d,0x%x,%d,data)\n",ID,start,bytes); |
slowness | 0:c03cffe402df | 905 | #endif |
slowness | 0:c03cffe402df | 906 | |
slowness | 0:c03cffe402df | 907 | // Build the TxPacket first in RAM, then we'll send in one go |
slowness | 0:c03cffe402df | 908 | #ifdef AX12_READ_DEBUG |
slowness | 0:c03cffe402df | 909 | printf("\nInstruction Packet\n Header : 0xFF, 0xFF\n"); |
slowness | 0:c03cffe402df | 910 | #endif |
slowness | 0:c03cffe402df | 911 | |
slowness | 0:c03cffe402df | 912 | TxBuf[0] = 0xff; |
slowness | 0:c03cffe402df | 913 | TxBuf[1] = 0xff; |
slowness | 0:c03cffe402df | 914 | |
slowness | 0:c03cffe402df | 915 | // ID |
slowness | 0:c03cffe402df | 916 | TxBuf[2] = ID; |
slowness | 0:c03cffe402df | 917 | sum += TxBuf[2]; |
slowness | 0:c03cffe402df | 918 | |
slowness | 0:c03cffe402df | 919 | #ifdef AX12_READ_DEBUG |
slowness | 0:c03cffe402df | 920 | printf(" ID : %d\n",TxBuf[2]); |
slowness | 0:c03cffe402df | 921 | #endif |
slowness | 0:c03cffe402df | 922 | |
slowness | 0:c03cffe402df | 923 | // Packet Length |
slowness | 0:c03cffe402df | 924 | TxBuf[3] = PacketLength+bytes; // Length = 4 ; 2 + 1 (start) = 1 (bytes) |
slowness | 0:c03cffe402df | 925 | sum += TxBuf[3]; // Accululate the packet sum |
slowness | 0:c03cffe402df | 926 | |
slowness | 0:c03cffe402df | 927 | #ifdef AX12_READ_DEBUG |
slowness | 0:c03cffe402df | 928 | printf(" Length : 0x%x\n",TxBuf[3]); |
slowness | 0:c03cffe402df | 929 | #endif |
slowness | 0:c03cffe402df | 930 | |
slowness | 0:c03cffe402df | 931 | // Instruction - Read |
slowness | 0:c03cffe402df | 932 | TxBuf[4] = 0x2; |
slowness | 0:c03cffe402df | 933 | sum += TxBuf[4]; |
slowness | 0:c03cffe402df | 934 | |
slowness | 0:c03cffe402df | 935 | #ifdef AX12_READ_DEBUG |
slowness | 0:c03cffe402df | 936 | printf(" Instruction : 0x%x\n",TxBuf[4]); |
slowness | 0:c03cffe402df | 937 | #endif |
slowness | 0:c03cffe402df | 938 | |
slowness | 0:c03cffe402df | 939 | // Start Address |
slowness | 0:c03cffe402df | 940 | TxBuf[5] = start; |
slowness | 0:c03cffe402df | 941 | sum += TxBuf[5]; |
slowness | 0:c03cffe402df | 942 | |
slowness | 0:c03cffe402df | 943 | #ifdef AX12_READ_DEBUG |
slowness | 0:c03cffe402df | 944 | printf(" Start Address : 0x%x\n",TxBuf[5]); |
slowness | 0:c03cffe402df | 945 | #endif |
slowness | 0:c03cffe402df | 946 | |
slowness | 0:c03cffe402df | 947 | // Bytes to read |
slowness | 0:c03cffe402df | 948 | TxBuf[6] = bytes; |
slowness | 0:c03cffe402df | 949 | sum += TxBuf[6]; |
slowness | 0:c03cffe402df | 950 | |
slowness | 0:c03cffe402df | 951 | #ifdef AX12_READ_DEBUG |
slowness | 0:c03cffe402df | 952 | printf(" No bytes : 0x%x\n",TxBuf[6]); |
slowness | 0:c03cffe402df | 953 | #endif |
slowness | 0:c03cffe402df | 954 | |
slowness | 0:c03cffe402df | 955 | // Checksum |
slowness | 0:c03cffe402df | 956 | TxBuf[7] = 0xFF - sum; |
slowness | 0:c03cffe402df | 957 | #ifdef AX12_READ_DEBUG |
slowness | 0:c03cffe402df | 958 | printf(" Checksum : 0x%x\n",TxBuf[7]); |
slowness | 0:c03cffe402df | 959 | #endif |
slowness | 0:c03cffe402df | 960 | /********************************************TRAME CONSTRUITE DANS TxBuf***************************************/ |
slowness | 0:c03cffe402df | 961 | |
slowness | 0:c03cffe402df | 962 | |
slowness | 0:c03cffe402df | 963 | |
slowness | 0:c03cffe402df | 964 | |
slowness | 0:c03cffe402df | 965 | /* Transmission de la trame construite precedemment dans le tableau TxBuf |
slowness | 0:c03cffe402df | 966 | */ |
slowness | 0:c03cffe402df | 967 | while ((timeout_transmit<1000) && (i < (7+bytes))) |
slowness | 0:c03cffe402df | 968 | { |
slowness | 0:c03cffe402df | 969 | if (_ax12.writeable()) |
slowness | 0:c03cffe402df | 970 | { |
slowness | 0:c03cffe402df | 971 | _ax12.putc(TxBuf[i]); |
slowness | 0:c03cffe402df | 972 | i++; |
slowness | 0:c03cffe402df | 973 | timeout_transmit = 0; |
slowness | 0:c03cffe402df | 974 | } |
slowness | 0:c03cffe402df | 975 | else timeout_transmit++; |
slowness | 0:c03cffe402df | 976 | } |
slowness | 0:c03cffe402df | 977 | |
slowness | 0:c03cffe402df | 978 | if (timeout_transmit == 1000 ) // dans le cas d'une sortie en timeout pour ne pas rester bloquer ! |
slowness | 0:c03cffe402df | 979 | { |
slowness | 0:c03cffe402df | 980 | #ifdef AX12_DEBUG |
slowness | 0:c03cffe402df | 981 | printf ("timeout transmit erreur\r\n"); |
slowness | 0:c03cffe402df | 982 | #endif |
slowness | 0:c03cffe402df | 983 | return(-1); |
slowness | 0:c03cffe402df | 984 | } |
slowness | 0:c03cffe402df | 985 | /* Transmission effectuée on va ensuite récuperer la trame de retour renvoyer par le servomoteur |
slowness | 0:c03cffe402df | 986 | */ |
slowness | 0:c03cffe402df | 987 | |
slowness | 0:c03cffe402df | 988 | |
slowness | 0:c03cffe402df | 989 | // Wait for the bytes to be transmitted |
slowness | 0:c03cffe402df | 990 | wait (0.001); |
slowness | 0:c03cffe402df | 991 | |
slowness | 0:c03cffe402df | 992 | |
slowness | 0:c03cffe402df | 993 | |
slowness | 0:c03cffe402df | 994 | // Skip if the read was to the broadcast address |
slowness | 0:c03cffe402df | 995 | if (_ID != 0xFE) { |
slowness | 0:c03cffe402df | 996 | |
slowness | 0:c03cffe402df | 997 | |
slowness | 0:c03cffe402df | 998 | |
slowness | 0:c03cffe402df | 999 | /* Partie de reception de la trame de retour |
slowness | 0:c03cffe402df | 1000 | */ |
slowness | 0:c03cffe402df | 1001 | while ((flag_out != 1) && (timeout < (1000*bytes))) |
slowness | 0:c03cffe402df | 1002 | { |
slowness | 0:c03cffe402df | 1003 | // Les differents etats de l'automate on été créés au debut de la fonction write ! |
slowness | 0:c03cffe402df | 1004 | switch (etat) |
slowness | 0:c03cffe402df | 1005 | { |
slowness | 0:c03cffe402df | 1006 | case Header1: if (_ax12.readable()) // reception du premier Header ( 0xFF ) |
slowness | 0:c03cffe402df | 1007 | { |
slowness | 0:c03cffe402df | 1008 | Status[plen] = _ax12.getc(); |
slowness | 0:c03cffe402df | 1009 | timeout = 0; |
slowness | 0:c03cffe402df | 1010 | if (Status[plen] == 0xFF ) |
slowness | 0:c03cffe402df | 1011 | { |
slowness | 0:c03cffe402df | 1012 | etat = Header2; |
slowness | 0:c03cffe402df | 1013 | #ifdef AX12_DEBUG_READ |
slowness | 0:c03cffe402df | 1014 | printf("data[%d] : %d\r\n", plen, (int)Status[plen]); |
slowness | 0:c03cffe402df | 1015 | #endif |
slowness | 0:c03cffe402df | 1016 | plen++; |
slowness | 0:c03cffe402df | 1017 | |
slowness | 0:c03cffe402df | 1018 | } |
slowness | 0:c03cffe402df | 1019 | else etat = Header1; |
slowness | 0:c03cffe402df | 1020 | } |
slowness | 0:c03cffe402df | 1021 | else timeout++; |
slowness | 0:c03cffe402df | 1022 | break; |
slowness | 0:c03cffe402df | 1023 | |
slowness | 0:c03cffe402df | 1024 | |
slowness | 0:c03cffe402df | 1025 | case Header2: if (_ax12.readable()) // reception du second Header ( 0xFF ) |
slowness | 0:c03cffe402df | 1026 | { |
slowness | 0:c03cffe402df | 1027 | Status[plen] = _ax12.getc(); |
slowness | 0:c03cffe402df | 1028 | timeout = 0; |
slowness | 0:c03cffe402df | 1029 | if (Status[plen] == 0xFF ) |
slowness | 0:c03cffe402df | 1030 | { |
slowness | 0:c03cffe402df | 1031 | etat = ident; |
slowness | 0:c03cffe402df | 1032 | #ifdef AX12_DEBUG_READ |
slowness | 0:c03cffe402df | 1033 | printf("data[%d] : %d\r\n", plen, (int)Status[plen]); |
slowness | 0:c03cffe402df | 1034 | #endif |
slowness | 0:c03cffe402df | 1035 | plen++; |
slowness | 0:c03cffe402df | 1036 | |
slowness | 0:c03cffe402df | 1037 | } |
slowness | 0:c03cffe402df | 1038 | else if (Status[plen] == ID ) // PERMET D'EVITER CERTAINES ERREUR LORSQU'ON LIT PLUSIEURS REGISTRES !!!! |
slowness | 0:c03cffe402df | 1039 | { |
slowness | 0:c03cffe402df | 1040 | Status[plen] = 0; |
slowness | 0:c03cffe402df | 1041 | #ifdef AX12_DEBUG_READ |
slowness | 0:c03cffe402df | 1042 | printf("data[%d] : %d\r\n", plen, (int)Status[plen]); |
slowness | 0:c03cffe402df | 1043 | #endif |
slowness | 0:c03cffe402df | 1044 | plen++; |
slowness | 0:c03cffe402df | 1045 | Status[plen] = ID; |
slowness | 0:c03cffe402df | 1046 | etat = length; |
slowness | 0:c03cffe402df | 1047 | #ifdef AX12_DEBUG_READ |
slowness | 0:c03cffe402df | 1048 | printf("data[%d] : %d\r\n", plen, (int)Status[plen]); |
slowness | 0:c03cffe402df | 1049 | #endif |
slowness | 0:c03cffe402df | 1050 | plen++; |
slowness | 0:c03cffe402df | 1051 | |
slowness | 0:c03cffe402df | 1052 | } |
slowness | 0:c03cffe402df | 1053 | else |
slowness | 0:c03cffe402df | 1054 | { |
slowness | 0:c03cffe402df | 1055 | |
slowness | 0:c03cffe402df | 1056 | etat = Header1; |
slowness | 0:c03cffe402df | 1057 | plen = 0; |
slowness | 0:c03cffe402df | 1058 | } |
slowness | 0:c03cffe402df | 1059 | } |
slowness | 0:c03cffe402df | 1060 | else timeout++; |
slowness | 0:c03cffe402df | 1061 | break; |
slowness | 0:c03cffe402df | 1062 | |
slowness | 0:c03cffe402df | 1063 | case ident: if (_ax12.readable()) // reception de l'octet correspondant à l'ID du servomoteur |
slowness | 0:c03cffe402df | 1064 | { |
slowness | 0:c03cffe402df | 1065 | Status[plen] = _ax12.getc(); |
slowness | 0:c03cffe402df | 1066 | timeout = 0; |
slowness | 0:c03cffe402df | 1067 | if (Status[plen] == ID ) |
slowness | 0:c03cffe402df | 1068 | { |
slowness | 0:c03cffe402df | 1069 | etat = length; |
slowness | 0:c03cffe402df | 1070 | #ifdef AX12_DEBUG_READ |
slowness | 0:c03cffe402df | 1071 | printf("data[%d] : %d\r\n", plen, (int)Status[plen]); |
slowness | 0:c03cffe402df | 1072 | #endif |
slowness | 0:c03cffe402df | 1073 | plen++; |
slowness | 0:c03cffe402df | 1074 | |
slowness | 0:c03cffe402df | 1075 | } |
slowness | 0:c03cffe402df | 1076 | else |
slowness | 0:c03cffe402df | 1077 | { |
slowness | 0:c03cffe402df | 1078 | etat = Header1; |
slowness | 0:c03cffe402df | 1079 | plen = 0; |
slowness | 0:c03cffe402df | 1080 | } |
slowness | 0:c03cffe402df | 1081 | } |
slowness | 0:c03cffe402df | 1082 | else timeout++; |
slowness | 0:c03cffe402df | 1083 | break; |
slowness | 0:c03cffe402df | 1084 | |
slowness | 0:c03cffe402df | 1085 | case length: if (_ax12.readable()) // reception de l'octet correspondant à la taille ( taille = 2 + nombre de paramètres ) |
slowness | 0:c03cffe402df | 1086 | { |
slowness | 0:c03cffe402df | 1087 | Status[plen] = _ax12.getc(); |
slowness | 0:c03cffe402df | 1088 | timeout = 0; |
slowness | 0:c03cffe402df | 1089 | if (Status[plen] == (bytes+2) ) |
slowness | 0:c03cffe402df | 1090 | { |
slowness | 0:c03cffe402df | 1091 | etat = erreur; |
slowness | 0:c03cffe402df | 1092 | #ifdef AX12_DEBUG_READ |
slowness | 0:c03cffe402df | 1093 | printf("data[%d] : %d\r\n", plen, (int)Status[plen]); |
slowness | 0:c03cffe402df | 1094 | #endif |
slowness | 0:c03cffe402df | 1095 | plen++; |
slowness | 0:c03cffe402df | 1096 | |
slowness | 0:c03cffe402df | 1097 | } |
slowness | 0:c03cffe402df | 1098 | else |
slowness | 0:c03cffe402df | 1099 | { |
slowness | 0:c03cffe402df | 1100 | etat = Header1; |
slowness | 0:c03cffe402df | 1101 | plen = 0; |
slowness | 0:c03cffe402df | 1102 | } |
slowness | 0:c03cffe402df | 1103 | } |
slowness | 0:c03cffe402df | 1104 | else timeout++; |
slowness | 0:c03cffe402df | 1105 | break; |
slowness | 0:c03cffe402df | 1106 | |
slowness | 0:c03cffe402df | 1107 | case erreur: if (_ax12.readable()) //reception de l'octet correspondant au code d'erreurs eventuels ( 0 = pas d'erreur ) |
slowness | 0:c03cffe402df | 1108 | { |
slowness | 0:c03cffe402df | 1109 | Status[plen] = _ax12.getc(); |
slowness | 0:c03cffe402df | 1110 | timeout = 0; |
slowness | 0:c03cffe402df | 1111 | #ifdef AX12_DEBUG_READ |
slowness | 0:c03cffe402df | 1112 | printf("data[%d] : %d\r\n", plen, (int)Status[plen]); |
slowness | 0:c03cffe402df | 1113 | #endif |
slowness | 0:c03cffe402df | 1114 | plen++; |
slowness | 0:c03cffe402df | 1115 | |
slowness | 0:c03cffe402df | 1116 | etat = reception; |
slowness | 0:c03cffe402df | 1117 | } |
slowness | 0:c03cffe402df | 1118 | else timeout++; |
slowness | 0:c03cffe402df | 1119 | |
slowness | 0:c03cffe402df | 1120 | case reception: while ( enable < bytes ) // reception du ou des octect(s) de donnés ( suivant la valeur de la variable length ) |
slowness | 0:c03cffe402df | 1121 | { |
slowness | 0:c03cffe402df | 1122 | if (_ax12.readable()) |
slowness | 0:c03cffe402df | 1123 | { |
slowness | 0:c03cffe402df | 1124 | Status[plen] = _ax12.getc(); |
slowness | 0:c03cffe402df | 1125 | timeout = 0; |
slowness | 0:c03cffe402df | 1126 | #ifdef AX12_DEBUG_READ |
slowness | 0:c03cffe402df | 1127 | printf("data[%d] : %d\r\n", plen, (int)Status[plen]); |
slowness | 0:c03cffe402df | 1128 | #endif |
slowness | 0:c03cffe402df | 1129 | plen++; |
slowness | 0:c03cffe402df | 1130 | enable++; |
slowness | 0:c03cffe402df | 1131 | |
slowness | 0:c03cffe402df | 1132 | } |
slowness | 0:c03cffe402df | 1133 | else timeout++; |
slowness | 0:c03cffe402df | 1134 | } |
slowness | 0:c03cffe402df | 1135 | etat = checksum; |
slowness | 0:c03cffe402df | 1136 | break; |
slowness | 0:c03cffe402df | 1137 | |
slowness | 0:c03cffe402df | 1138 | case checksum: if (_ax12.readable()) // reception du dernier octet ( Checksum ) >>> checksum = NOT ( ID + length + somme des données ) >>>> dans le cas d'un retour d'un read!! |
slowness | 0:c03cffe402df | 1139 | { |
slowness | 0:c03cffe402df | 1140 | Status[plen] = _ax12.getc(); |
slowness | 0:c03cffe402df | 1141 | timeout = 0; |
slowness | 0:c03cffe402df | 1142 | flag_out = 1; |
slowness | 0:c03cffe402df | 1143 | etat = Header1; |
slowness | 0:c03cffe402df | 1144 | |
slowness | 0:c03cffe402df | 1145 | #ifdef AX12_DEBUG_READ |
slowness | 0:c03cffe402df | 1146 | printf("data[%d] : %d\r\n\n", plen, (int)Status[plen]); |
slowness | 0:c03cffe402df | 1147 | #endif |
slowness | 0:c03cffe402df | 1148 | } |
slowness | 0:c03cffe402df | 1149 | else timeout++; |
slowness | 0:c03cffe402df | 1150 | break; |
slowness | 0:c03cffe402df | 1151 | |
slowness | 0:c03cffe402df | 1152 | default: break; |
slowness | 0:c03cffe402df | 1153 | } |
slowness | 0:c03cffe402df | 1154 | } |
slowness | 0:c03cffe402df | 1155 | |
slowness | 0:c03cffe402df | 1156 | |
slowness | 0:c03cffe402df | 1157 | if (timeout == (1000*bytes) ) // permet d'afficher si il y a une erreur de timeout et de ne pas rester bloquer si il y a des erreurs de trames |
slowness | 0:c03cffe402df | 1158 | { |
slowness | 0:c03cffe402df | 1159 | #ifdef AX12_DEBUG |
slowness | 0:c03cffe402df | 1160 | printf ("timeout erreur\n"); |
slowness | 0:c03cffe402df | 1161 | #endif |
slowness | 0:c03cffe402df | 1162 | return(-1); |
slowness | 0:c03cffe402df | 1163 | } |
slowness | 0:c03cffe402df | 1164 | |
slowness | 0:c03cffe402df | 1165 | |
slowness | 0:c03cffe402df | 1166 | // copie des données dans le tableau data |
slowness | 0:c03cffe402df | 1167 | for (int i=0; i < Status[3]-2 ; i++) |
slowness | 0:c03cffe402df | 1168 | { |
slowness | 0:c03cffe402df | 1169 | data[i] = Status[5+i]; |
slowness | 0:c03cffe402df | 1170 | } |
slowness | 0:c03cffe402df | 1171 | |
slowness | 0:c03cffe402df | 1172 | } // toute la partie precedente ne s'effectue pas dans le cas d'un appel avec un broadcast ID (ID!=0xFE) |
slowness | 0:c03cffe402df | 1173 | |
slowness | 0:c03cffe402df | 1174 | return(Status[4]); // retourne le code d'erreur ( octect 5 de la trame de retour ) |
slowness | 0:c03cffe402df | 1175 | } |
slowness | 0:c03cffe402df | 1176 | |
slowness | 0:c03cffe402df | 1177 | int AX12::write(int ID, int start, int bytes, char* data, int flag) |
slowness | 0:c03cffe402df | 1178 | { |
slowness | 0:c03cffe402df | 1179 | // 0xff, 0xff, ID, Length, Intruction(write), Address, Param(s), Checksum |
slowness | 0:c03cffe402df | 1180 | |
slowness | 0:c03cffe402df | 1181 | char TxBuf[16]; |
slowness | 0:c03cffe402df | 1182 | char sum = 0; |
slowness | 0:c03cffe402df | 1183 | char Status[6]; |
slowness | 0:c03cffe402df | 1184 | |
slowness | 0:c03cffe402df | 1185 | int timeout = 0; |
slowness | 0:c03cffe402df | 1186 | int plen = 0; |
slowness | 0:c03cffe402df | 1187 | int flag_out = 0; |
slowness | 0:c03cffe402df | 1188 | int timeout_transmit = 0; |
slowness | 0:c03cffe402df | 1189 | int i = 0; |
slowness | 0:c03cffe402df | 1190 | int poubelle = 0; |
slowness | 0:c03cffe402df | 1191 | int count = 0; |
slowness | 0:c03cffe402df | 1192 | char vidage[50]; |
slowness | 0:c03cffe402df | 1193 | |
slowness | 0:c03cffe402df | 1194 | typedef enum {Header1, Header2, ident, length, erreur, checksum} type_etat; |
slowness | 0:c03cffe402df | 1195 | type_etat etat = Header1; |
slowness | 0:c03cffe402df | 1196 | |
slowness | 0:c03cffe402df | 1197 | #ifdef AX12_WRITE_DEBUG |
slowness | 0:c03cffe402df | 1198 | printf("\nwrite(%d,0x%x,%d,data,%d)\n",ID,start,bytes,flag); |
slowness | 0:c03cffe402df | 1199 | #endif |
slowness | 0:c03cffe402df | 1200 | |
slowness | 0:c03cffe402df | 1201 | // Build the TxPacket first in RAM, then we'll send in one go |
slowness | 0:c03cffe402df | 1202 | #ifdef AX12_WRITE_DEBUG |
slowness | 0:c03cffe402df | 1203 | printf("\nInstruction Packet\n Header : 0xFF, 0xFF\n"); |
slowness | 0:c03cffe402df | 1204 | #endif |
slowness | 0:c03cffe402df | 1205 | |
slowness | 0:c03cffe402df | 1206 | TxBuf[0] = 0xff; |
slowness | 0:c03cffe402df | 1207 | TxBuf[1] = 0xff; |
slowness | 0:c03cffe402df | 1208 | |
slowness | 0:c03cffe402df | 1209 | // ID |
slowness | 0:c03cffe402df | 1210 | TxBuf[2] = ID; |
slowness | 0:c03cffe402df | 1211 | sum += TxBuf[2]; |
slowness | 0:c03cffe402df | 1212 | |
slowness | 0:c03cffe402df | 1213 | #ifdef AX12_WRITE_DEBUG |
slowness | 0:c03cffe402df | 1214 | printf(" ID : %d\n",TxBuf[2]); |
slowness | 0:c03cffe402df | 1215 | #endif |
slowness | 0:c03cffe402df | 1216 | |
slowness | 0:c03cffe402df | 1217 | // packet Length |
slowness | 0:c03cffe402df | 1218 | TxBuf[3] = 3+bytes; |
slowness | 0:c03cffe402df | 1219 | sum += TxBuf[3]; |
slowness | 0:c03cffe402df | 1220 | |
slowness | 0:c03cffe402df | 1221 | #ifdef AX12_WRITE_DEBUG |
slowness | 0:c03cffe402df | 1222 | printf(" Length : %d\n",TxBuf[3]); |
slowness | 0:c03cffe402df | 1223 | #endif |
slowness | 0:c03cffe402df | 1224 | |
slowness | 0:c03cffe402df | 1225 | // Instruction |
slowness | 0:c03cffe402df | 1226 | if (flag == 1) { |
slowness | 0:c03cffe402df | 1227 | TxBuf[4]=0x04; |
slowness | 0:c03cffe402df | 1228 | sum += TxBuf[4]; |
slowness | 0:c03cffe402df | 1229 | } else { |
slowness | 0:c03cffe402df | 1230 | TxBuf[4]=0x03; |
slowness | 0:c03cffe402df | 1231 | sum += TxBuf[4]; |
slowness | 0:c03cffe402df | 1232 | } |
slowness | 0:c03cffe402df | 1233 | |
slowness | 0:c03cffe402df | 1234 | #ifdef AX12_WRITE_DEBUG |
slowness | 0:c03cffe402df | 1235 | printf(" Instruction : 0x%x\n",TxBuf[4]); |
slowness | 0:c03cffe402df | 1236 | #endif |
slowness | 0:c03cffe402df | 1237 | |
slowness | 0:c03cffe402df | 1238 | // Start Address |
slowness | 0:c03cffe402df | 1239 | TxBuf[5] = start; |
slowness | 0:c03cffe402df | 1240 | sum += TxBuf[5]; |
slowness | 0:c03cffe402df | 1241 | |
slowness | 0:c03cffe402df | 1242 | #ifdef AX12_WRITE_DEBUG |
slowness | 0:c03cffe402df | 1243 | printf(" Start : 0x%x\n",TxBuf[5]); |
slowness | 0:c03cffe402df | 1244 | #endif |
slowness | 0:c03cffe402df | 1245 | |
slowness | 0:c03cffe402df | 1246 | // data |
slowness | 0:c03cffe402df | 1247 | for (char i=0; i<bytes ; i++) { |
slowness | 0:c03cffe402df | 1248 | TxBuf[6+i] = data[i]; |
slowness | 0:c03cffe402df | 1249 | sum += TxBuf[6+i]; |
slowness | 0:c03cffe402df | 1250 | |
slowness | 0:c03cffe402df | 1251 | #ifdef AX12_WRITE_DEBUG |
slowness | 0:c03cffe402df | 1252 | printf(" Data : 0x%x\n",TxBuf[6+i]); |
slowness | 0:c03cffe402df | 1253 | #endif |
slowness | 0:c03cffe402df | 1254 | |
slowness | 0:c03cffe402df | 1255 | } |
slowness | 0:c03cffe402df | 1256 | |
slowness | 0:c03cffe402df | 1257 | // checksum |
slowness | 0:c03cffe402df | 1258 | TxBuf[6+bytes] = 0xFF - sum; |
slowness | 0:c03cffe402df | 1259 | |
slowness | 0:c03cffe402df | 1260 | #ifdef AX12_WRITE_DEBUG |
slowness | 0:c03cffe402df | 1261 | printf(" Checksum : 0x%x\n",TxBuf[6+bytes]); |
slowness | 0:c03cffe402df | 1262 | #endif |
slowness | 0:c03cffe402df | 1263 | |
slowness | 0:c03cffe402df | 1264 | |
slowness | 0:c03cffe402df | 1265 | /* Transmission de la trame construite precedemment dans le tableau TxBuf |
slowness | 0:c03cffe402df | 1266 | */ |
slowness | 0:c03cffe402df | 1267 | while ((timeout_transmit<100) && (i < (7+bytes))) |
slowness | 0:c03cffe402df | 1268 | { |
slowness | 0:c03cffe402df | 1269 | if (_ax12.writeable()) |
slowness | 0:c03cffe402df | 1270 | { |
slowness | 0:c03cffe402df | 1271 | _ax12.putc(TxBuf[i]); |
slowness | 0:c03cffe402df | 1272 | i++; |
slowness | 0:c03cffe402df | 1273 | timeout_transmit = 0; |
slowness | 0:c03cffe402df | 1274 | } |
slowness | 0:c03cffe402df | 1275 | else timeout_transmit++; |
slowness | 0:c03cffe402df | 1276 | } |
slowness | 0:c03cffe402df | 1277 | |
slowness | 0:c03cffe402df | 1278 | if (timeout_transmit == 100 ) // dans le cas d'une sortie en timeout pour ne pas rester bloquer ! |
slowness | 0:c03cffe402df | 1279 | { |
slowness | 0:c03cffe402df | 1280 | #ifdef AX12_DEBUG |
slowness | 0:c03cffe402df | 1281 | printf ("TIMEOUT TRANSMIT ERROR\r\n"); |
slowness | 0:c03cffe402df | 1282 | #endif |
slowness | 0:c03cffe402df | 1283 | return(-1); |
slowness | 0:c03cffe402df | 1284 | } |
slowness | 0:c03cffe402df | 1285 | /* Transmission effectuée on va ensuite récuperer la trame de retour renvoyer par le servomoteur |
slowness | 0:c03cffe402df | 1286 | */ |
slowness | 0:c03cffe402df | 1287 | |
slowness | 0:c03cffe402df | 1288 | |
slowness | 0:c03cffe402df | 1289 | // Wait for data to transmit |
slowness | 0:c03cffe402df | 1290 | wait (0.005); |
slowness | 0:c03cffe402df | 1291 | |
slowness | 0:c03cffe402df | 1292 | // make sure we have a valid return |
slowness | 0:c03cffe402df | 1293 | Status[4]=0x00; |
slowness | 0:c03cffe402df | 1294 | |
slowness | 0:c03cffe402df | 1295 | // we'll only get a reply if it was not broadcast |
slowness | 0:c03cffe402df | 1296 | if (_ID!=0xFE) { |
slowness | 0:c03cffe402df | 1297 | |
slowness | 0:c03cffe402df | 1298 | |
slowness | 0:c03cffe402df | 1299 | /* Partie de reception de la trame de retour |
slowness | 0:c03cffe402df | 1300 | */ |
slowness | 0:c03cffe402df | 1301 | while ((flag_out != 1) && (timeout < MAX_TIMEOUT)) |
slowness | 0:c03cffe402df | 1302 | { |
slowness | 0:c03cffe402df | 1303 | // Les differents etats de l'automate on été créés au debut de la fonction write ! |
slowness | 0:c03cffe402df | 1304 | switch (etat) |
slowness | 0:c03cffe402df | 1305 | { |
slowness | 0:c03cffe402df | 1306 | case Header1: if (_ax12.readable()) // reception du premier Header ( 0xFF ) |
slowness | 0:c03cffe402df | 1307 | { |
slowness | 0:c03cffe402df | 1308 | Status[plen] = _ax12.getc(); |
slowness | 0:c03cffe402df | 1309 | timeout = 0; |
slowness | 0:c03cffe402df | 1310 | if (Status[plen] == 0xFF ) |
slowness | 0:c03cffe402df | 1311 | { |
slowness | 0:c03cffe402df | 1312 | etat = Header2; |
slowness | 0:c03cffe402df | 1313 | #ifdef AX12_DEBUG_WRITE |
slowness | 0:c03cffe402df | 1314 | printf("data[%d] : %d\r\n", plen, (int)Status[plen]); |
slowness | 0:c03cffe402df | 1315 | #endif |
slowness | 0:c03cffe402df | 1316 | plen++; |
slowness | 0:c03cffe402df | 1317 | |
slowness | 0:c03cffe402df | 1318 | } |
slowness | 0:c03cffe402df | 1319 | else etat = Header1; |
slowness | 0:c03cffe402df | 1320 | } |
slowness | 0:c03cffe402df | 1321 | else timeout++; |
slowness | 0:c03cffe402df | 1322 | break; |
slowness | 0:c03cffe402df | 1323 | |
slowness | 0:c03cffe402df | 1324 | |
slowness | 0:c03cffe402df | 1325 | case Header2: if (_ax12.readable()) // reception du second Header ( 0xFF ) |
slowness | 0:c03cffe402df | 1326 | { |
slowness | 0:c03cffe402df | 1327 | Status[plen] = _ax12.getc(); |
slowness | 0:c03cffe402df | 1328 | timeout = 0; |
slowness | 0:c03cffe402df | 1329 | if (Status[plen] == 0xFF ) |
slowness | 0:c03cffe402df | 1330 | { |
slowness | 0:c03cffe402df | 1331 | etat = ident; |
slowness | 0:c03cffe402df | 1332 | #ifdef AX12_DEBUG_WRITE |
slowness | 0:c03cffe402df | 1333 | printf("data[%d] : %d\r\n", plen, (int)Status[plen]); |
slowness | 0:c03cffe402df | 1334 | #endif |
slowness | 0:c03cffe402df | 1335 | plen++; |
slowness | 0:c03cffe402df | 1336 | } |
slowness | 0:c03cffe402df | 1337 | else |
slowness | 0:c03cffe402df | 1338 | { |
slowness | 0:c03cffe402df | 1339 | etat = Header1; |
slowness | 0:c03cffe402df | 1340 | plen = 0; |
slowness | 0:c03cffe402df | 1341 | } |
slowness | 0:c03cffe402df | 1342 | } |
slowness | 0:c03cffe402df | 1343 | else timeout++; |
slowness | 0:c03cffe402df | 1344 | break; |
slowness | 0:c03cffe402df | 1345 | |
slowness | 0:c03cffe402df | 1346 | case ident: if (_ax12.readable()) // reception de l'octet correspondant à l'ID du servomoteur |
slowness | 0:c03cffe402df | 1347 | { |
slowness | 0:c03cffe402df | 1348 | Status[plen] = _ax12.getc(); |
slowness | 0:c03cffe402df | 1349 | timeout = 0; |
slowness | 0:c03cffe402df | 1350 | if (Status[plen] == ID ) |
slowness | 0:c03cffe402df | 1351 | { |
slowness | 0:c03cffe402df | 1352 | etat = length; |
slowness | 0:c03cffe402df | 1353 | #ifdef AX12_DEBUG_WRITE |
slowness | 0:c03cffe402df | 1354 | printf("data[%d] : %d\r\n", plen, (int)Status[plen]); |
slowness | 0:c03cffe402df | 1355 | #endif |
slowness | 0:c03cffe402df | 1356 | plen++; |
slowness | 0:c03cffe402df | 1357 | } |
slowness | 0:c03cffe402df | 1358 | else |
slowness | 0:c03cffe402df | 1359 | { |
slowness | 0:c03cffe402df | 1360 | etat = Header1; |
slowness | 0:c03cffe402df | 1361 | plen = 0; |
slowness | 0:c03cffe402df | 1362 | } |
slowness | 0:c03cffe402df | 1363 | } |
slowness | 0:c03cffe402df | 1364 | else timeout++; |
slowness | 0:c03cffe402df | 1365 | break; |
slowness | 0:c03cffe402df | 1366 | |
slowness | 0:c03cffe402df | 1367 | case length: if (_ax12.readable()) // reception de l'octet correspondant à la taille ( taille = 2 + nombre de paramètres ) |
slowness | 0:c03cffe402df | 1368 | { |
slowness | 0:c03cffe402df | 1369 | Status[plen] = _ax12.getc(); |
slowness | 0:c03cffe402df | 1370 | timeout = 0; |
slowness | 0:c03cffe402df | 1371 | if (Status[plen] == 2 ) // dans la trame de retour d'un write il n'y a pas de paramètre la taille vaudra donc 2!! |
slowness | 0:c03cffe402df | 1372 | { |
slowness | 0:c03cffe402df | 1373 | etat = erreur; |
slowness | 0:c03cffe402df | 1374 | #ifdef AX12_DEBUG_WRITE |
slowness | 0:c03cffe402df | 1375 | printf("data[%d] : %d\r\n", plen, (int)Status[plen]); |
slowness | 0:c03cffe402df | 1376 | #endif |
slowness | 0:c03cffe402df | 1377 | plen++; |
slowness | 0:c03cffe402df | 1378 | } |
slowness | 0:c03cffe402df | 1379 | else |
slowness | 0:c03cffe402df | 1380 | { |
slowness | 0:c03cffe402df | 1381 | etat = Header1; |
slowness | 0:c03cffe402df | 1382 | plen = 0; |
slowness | 0:c03cffe402df | 1383 | } |
slowness | 0:c03cffe402df | 1384 | } |
slowness | 0:c03cffe402df | 1385 | else timeout++; |
slowness | 0:c03cffe402df | 1386 | break; |
slowness | 0:c03cffe402df | 1387 | |
slowness | 0:c03cffe402df | 1388 | case erreur: if (_ax12.readable()) //reception de l'octet correspondant au code d'erreurs eventuels ( 0 = pas d'erreur ) |
slowness | 0:c03cffe402df | 1389 | { |
slowness | 0:c03cffe402df | 1390 | Status[plen] = _ax12.getc(); |
slowness | 0:c03cffe402df | 1391 | timeout = 0; |
slowness | 0:c03cffe402df | 1392 | #ifdef AX12_DEBUG_WRITE |
slowness | 0:c03cffe402df | 1393 | printf("data[%d] : %d\r\n", plen, (int)Status[plen]); |
slowness | 0:c03cffe402df | 1394 | #endif |
slowness | 0:c03cffe402df | 1395 | plen++; |
slowness | 0:c03cffe402df | 1396 | etat = checksum; |
slowness | 0:c03cffe402df | 1397 | } |
slowness | 0:c03cffe402df | 1398 | else timeout++; |
slowness | 0:c03cffe402df | 1399 | |
slowness | 0:c03cffe402df | 1400 | case checksum: if (_ax12.readable()) // recpetion du dernier octet ( Checksum ) >>> checksum = NOT ( ID + length ) >>>> dans le cas de la reception d'un write |
slowness | 0:c03cffe402df | 1401 | { |
slowness | 0:c03cffe402df | 1402 | Status[plen] = _ax12.getc(); |
slowness | 0:c03cffe402df | 1403 | timeout = 0; |
slowness | 0:c03cffe402df | 1404 | flag_out = 1; |
slowness | 0:c03cffe402df | 1405 | etat = Header1; |
slowness | 0:c03cffe402df | 1406 | #ifdef AX12_DEBUG_WRITE |
slowness | 0:c03cffe402df | 1407 | printf("data[%d] : %d\r\n\n", plen, (int)Status[plen]); |
slowness | 0:c03cffe402df | 1408 | #endif |
slowness | 0:c03cffe402df | 1409 | } |
slowness | 0:c03cffe402df | 1410 | else timeout++; |
slowness | 0:c03cffe402df | 1411 | break; |
slowness | 0:c03cffe402df | 1412 | } |
slowness | 0:c03cffe402df | 1413 | } |
slowness | 0:c03cffe402df | 1414 | |
slowness | 0:c03cffe402df | 1415 | |
slowness | 0:c03cffe402df | 1416 | if ( Status[4] != 0 ) |
slowness | 0:c03cffe402df | 1417 | { |
slowness | 0:c03cffe402df | 1418 | #ifdef AX12_DEBUG |
slowness | 0:c03cffe402df | 1419 | printf ("erreur ! \r\n"); |
slowness | 0:c03cffe402df | 1420 | #endif |
slowness | 0:c03cffe402df | 1421 | for (int i = 0; i<5; i++) |
slowness | 0:c03cffe402df | 1422 | { |
slowness | 0:c03cffe402df | 1423 | #ifdef AX12_DEBUG |
slowness | 0:c03cffe402df | 1424 | printf("data[%d] : %d\r\n", plen, (int)Status[plen]); |
slowness | 0:c03cffe402df | 1425 | #endif |
slowness | 0:c03cffe402df | 1426 | } |
slowness | 0:c03cffe402df | 1427 | } |
slowness | 0:c03cffe402df | 1428 | |
slowness | 0:c03cffe402df | 1429 | if (timeout == MAX_TIMEOUT ) // permet d'afficher si il y a une erreur de timeout et de ne pas rester bloquer si il y a des erreurs de trames |
slowness | 0:c03cffe402df | 1430 | { |
slowness | 0:c03cffe402df | 1431 | #ifdef AX12_DEBUG |
slowness | 0:c03cffe402df | 1432 | printf ("timeout erreur\n\r"); |
slowness | 0:c03cffe402df | 1433 | #endif |
slowness | 0:c03cffe402df | 1434 | return(-1); |
slowness | 0:c03cffe402df | 1435 | } |
slowness | 0:c03cffe402df | 1436 | |
slowness | 0:c03cffe402df | 1437 | // Build the TxPacket first in RAM, then we'll send in one go |
slowness | 0:c03cffe402df | 1438 | } |
slowness | 0:c03cffe402df | 1439 | |
slowness | 0:c03cffe402df | 1440 | return(Status[4]); // retourne le code d'erreur ( octect 5 de la trame de retour ) |
slowness | 0:c03cffe402df | 1441 | } |