config AX12
Fork of configure_ax12_test_bras_module by
AX12.cpp@0:c03cffe402df, 2016-02-03 (annotated)
- Committer:
- slowness
- Date:
- Wed Feb 03 14:01:53 2016 +0000
- Revision:
- 0:c03cffe402df
- Child:
- 1:fce3323d874f
Pour configurer les AX12 avec la carte NXP1768 sur les Pin 9 et 10
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 | 0:c03cffe402df | 459 | |
slowness | 0:c03cffe402df | 460 | // if flag[0] is set, were blocking |
slowness | 0:c03cffe402df | 461 | // if flag[1] is set, we're registering |
slowness | 0:c03cffe402df | 462 | // they are mutually exclusive operations |
slowness | 0:c03cffe402df | 463 | int AX12::Set_Goal(int degrees, int flags) |
slowness | 0:c03cffe402df | 464 | { |
slowness | 0:c03cffe402df | 465 | |
slowness | 0:c03cffe402df | 466 | char reg_flag = 0; |
slowness | 0:c03cffe402df | 467 | char data[2]; |
slowness | 0:c03cffe402df | 468 | |
slowness | 0:c03cffe402df | 469 | // set the flag is only the register bit is set in the flag |
slowness | 0:c03cffe402df | 470 | if (flags == 0x2) { |
slowness | 0:c03cffe402df | 471 | reg_flag = 1; |
slowness | 0:c03cffe402df | 472 | } |
slowness | 0:c03cffe402df | 473 | |
slowness | 0:c03cffe402df | 474 | // 1023 / 300 * degrees |
slowness | 0:c03cffe402df | 475 | short goal = (1023 * degrees) / 300; |
slowness | 0:c03cffe402df | 476 | #ifdef AX12_DEBUG |
slowness | 0:c03cffe402df | 477 | printf("SetGoal to 0x%x\n",goal); |
slowness | 0:c03cffe402df | 478 | #endif |
slowness | 0:c03cffe402df | 479 | |
slowness | 0:c03cffe402df | 480 | data[0] = goal & 0xff; // bottom 8 bits |
slowness | 0:c03cffe402df | 481 | data[1] = goal >> 8; // top 8 bits |
slowness | 0:c03cffe402df | 482 | |
slowness | 0:c03cffe402df | 483 | // write the packet, return the error code |
slowness | 0:c03cffe402df | 484 | int rVal = write(_ID, AX12_REG_GOAL_POSITION, 2, data, reg_flag); |
slowness | 0:c03cffe402df | 485 | |
slowness | 0:c03cffe402df | 486 | /*if (flags == 1) { |
slowness | 0:c03cffe402df | 487 | // block until it comes to a halt |
slowness | 0:c03cffe402df | 488 | while (isMoving()) |
slowness | 0:c03cffe402df | 489 | { |
slowness | 0:c03cffe402df | 490 | } |
slowness | 0:c03cffe402df | 491 | |
slowness | 0:c03cffe402df | 492 | }*/ |
slowness | 0:c03cffe402df | 493 | return(rVal); |
slowness | 0:c03cffe402df | 494 | } |
slowness | 0:c03cffe402df | 495 | |
slowness | 0:c03cffe402df | 496 | |
slowness | 0:c03cffe402df | 497 | // Set continuous rotation speed from -1 to 1 |
slowness | 0:c03cffe402df | 498 | int AX12::Set_CR_Speed(float speed) |
slowness | 0:c03cffe402df | 499 | { |
slowness | 0:c03cffe402df | 500 | |
slowness | 0:c03cffe402df | 501 | // bit 10 = direction, 0 = CCW, 1=CW |
slowness | 0:c03cffe402df | 502 | // bits 9-0 = Speed |
slowness | 0:c03cffe402df | 503 | char data[2]; |
slowness | 0:c03cffe402df | 504 | |
slowness | 0:c03cffe402df | 505 | int goal = (0x3ff * abs(speed)); |
slowness | 0:c03cffe402df | 506 | |
slowness | 0:c03cffe402df | 507 | // Set direction CW if we have a negative speed |
slowness | 0:c03cffe402df | 508 | if (speed < 0) { |
slowness | 0:c03cffe402df | 509 | goal |= (0x1 << 10); |
slowness | 0:c03cffe402df | 510 | } |
slowness | 0:c03cffe402df | 511 | |
slowness | 0:c03cffe402df | 512 | data[0] = goal & 0xff; // bottom 8 bits |
slowness | 0:c03cffe402df | 513 | data[1] = goal >> 8; // top 8 bits |
slowness | 0:c03cffe402df | 514 | |
slowness | 0:c03cffe402df | 515 | // write the packet, return the error code |
slowness | 0:c03cffe402df | 516 | int rVal = write(_ID, 0x20, 2, data); |
slowness | 0:c03cffe402df | 517 | |
slowness | 0:c03cffe402df | 518 | return(rVal); |
slowness | 0:c03cffe402df | 519 | } |
slowness | 0:c03cffe402df | 520 | |
slowness | 0:c03cffe402df | 521 | |
slowness | 0:c03cffe402df | 522 | int AX12::Set_CW_Angle_Limit (int degrees) |
slowness | 0:c03cffe402df | 523 | { |
slowness | 0:c03cffe402df | 524 | |
slowness | 0:c03cffe402df | 525 | char data[2]; |
slowness | 0:c03cffe402df | 526 | |
slowness | 0:c03cffe402df | 527 | // 1023 / 300 * degrees |
slowness | 0:c03cffe402df | 528 | short limit = (1023 * degrees) / 300; |
slowness | 0:c03cffe402df | 529 | |
slowness | 0:c03cffe402df | 530 | #ifdef AX12_DEBUG |
slowness | 0:c03cffe402df | 531 | printf("SetCWLimit to 0x%x\n",limit); |
slowness | 0:c03cffe402df | 532 | #endif |
slowness | 0:c03cffe402df | 533 | |
slowness | 0:c03cffe402df | 534 | data[0] = limit & 0xff; // bottom 8 bits |
slowness | 0:c03cffe402df | 535 | data[1] = limit >> 8; // top 8 bits |
slowness | 0:c03cffe402df | 536 | |
slowness | 0:c03cffe402df | 537 | // write the packet, return the error code |
slowness | 0:c03cffe402df | 538 | return (write(_ID, AX12_REG_CW_LIMIT, 2, data)); |
slowness | 0:c03cffe402df | 539 | |
slowness | 0:c03cffe402df | 540 | } |
slowness | 0:c03cffe402df | 541 | |
slowness | 0:c03cffe402df | 542 | int AX12::Set_CCW_Angle_Limit (int degrees) |
slowness | 0:c03cffe402df | 543 | { |
slowness | 0:c03cffe402df | 544 | |
slowness | 0:c03cffe402df | 545 | char data[2]; |
slowness | 0:c03cffe402df | 546 | |
slowness | 0:c03cffe402df | 547 | // 1023 / 300 * degrees |
slowness | 0:c03cffe402df | 548 | short limit = (1023 * degrees) / 300; |
slowness | 0:c03cffe402df | 549 | |
slowness | 0:c03cffe402df | 550 | #ifdef AX12_DEBUG |
slowness | 0:c03cffe402df | 551 | printf("SetCCWLimit to 0x%x\n",limit); |
slowness | 0:c03cffe402df | 552 | #endif |
slowness | 0:c03cffe402df | 553 | |
slowness | 0:c03cffe402df | 554 | data[0] = limit & 0xff; // bottom 8 bits |
slowness | 0:c03cffe402df | 555 | data[1] = limit >> 8; // top 8 bits |
slowness | 0:c03cffe402df | 556 | |
slowness | 0:c03cffe402df | 557 | // write the packet, return the error code |
slowness | 0:c03cffe402df | 558 | return (write(_ID, AX12_REG_CCW_LIMIT, 2, data)); |
slowness | 0:c03cffe402df | 559 | } |
slowness | 0:c03cffe402df | 560 | |
slowness | 0:c03cffe402df | 561 | |
slowness | 0:c03cffe402df | 562 | int AX12::Set_ID (int CurrentID, int NewID) |
slowness | 0:c03cffe402df | 563 | { |
slowness | 0:c03cffe402df | 564 | |
slowness | 0:c03cffe402df | 565 | char data[1]; |
slowness | 0:c03cffe402df | 566 | data[0] = NewID; |
slowness | 0:c03cffe402df | 567 | |
slowness | 0:c03cffe402df | 568 | #ifdef AX12_DEBUG |
slowness | 0:c03cffe402df | 569 | printf("Setting ID from 0x%x to 0x%x\n",CurrentID,NewID); |
slowness | 0:c03cffe402df | 570 | #endif |
slowness | 0:c03cffe402df | 571 | |
slowness | 0:c03cffe402df | 572 | return (write(CurrentID, AX12_REG_ID, 1, data)); |
slowness | 0:c03cffe402df | 573 | |
slowness | 0:c03cffe402df | 574 | } |
slowness | 0:c03cffe402df | 575 | |
slowness | 0:c03cffe402df | 576 | |
slowness | 0:c03cffe402df | 577 | int AX12::Set_Baud (int baud) |
slowness | 0:c03cffe402df | 578 | { |
slowness | 0:c03cffe402df | 579 | |
slowness | 0:c03cffe402df | 580 | char data[1]; |
slowness | 0:c03cffe402df | 581 | data[0] = baud; |
slowness | 0:c03cffe402df | 582 | |
slowness | 0:c03cffe402df | 583 | #ifdef AX12_DEBUG |
slowness | 0:c03cffe402df | 584 | printf("Setting Baud rate to %d\n",baud); |
slowness | 0:c03cffe402df | 585 | #endif |
slowness | 0:c03cffe402df | 586 | |
slowness | 0:c03cffe402df | 587 | return (write(_ID, AX12_REG_BAUD, 1, data)); |
slowness | 0:c03cffe402df | 588 | |
slowness | 0:c03cffe402df | 589 | } |
slowness | 0:c03cffe402df | 590 | |
slowness | 0:c03cffe402df | 591 | |
slowness | 0:c03cffe402df | 592 | |
slowness | 0:c03cffe402df | 593 | // return 1 is the servo is still in flight |
slowness | 0:c03cffe402df | 594 | int AX12::isMoving(void) |
slowness | 0:c03cffe402df | 595 | { |
slowness | 0:c03cffe402df | 596 | |
slowness | 0:c03cffe402df | 597 | char data[1]; |
slowness | 0:c03cffe402df | 598 | read(_ID,AX12_REG_MOVING,1,data); |
slowness | 0:c03cffe402df | 599 | return(data[0]); |
slowness | 0:c03cffe402df | 600 | } |
slowness | 0:c03cffe402df | 601 | |
slowness | 0:c03cffe402df | 602 | void AX12::reset() |
slowness | 0:c03cffe402df | 603 | { |
slowness | 0:c03cffe402df | 604 | |
slowness | 0:c03cffe402df | 605 | unsigned char TxBuf[16]; |
slowness | 0:c03cffe402df | 606 | unsigned char sum = 0; |
slowness | 0:c03cffe402df | 607 | unsigned long debut=0; |
slowness | 0:c03cffe402df | 608 | |
slowness | 0:c03cffe402df | 609 | #ifdef AX12_TRIGGER_DEBUG |
slowness | 0:c03cffe402df | 610 | // Build the TxPacket first in RAM, then we'll send in one go |
slowness | 0:c03cffe402df | 611 | printf("\nreset\n"); |
slowness | 0:c03cffe402df | 612 | printf("\nreset Packet\n Header : 0xFF, 0xFF\n"); |
slowness | 0:c03cffe402df | 613 | #endif |
slowness | 0:c03cffe402df | 614 | |
slowness | 0:c03cffe402df | 615 | TxBuf[0] = 0xFF; |
slowness | 0:c03cffe402df | 616 | TxBuf[1] = 0xFF; |
slowness | 0:c03cffe402df | 617 | |
slowness | 0:c03cffe402df | 618 | // ID - Broadcast |
slowness | 0:c03cffe402df | 619 | TxBuf[2] =_ID; |
slowness | 0:c03cffe402df | 620 | sum += TxBuf[2]; |
slowness | 0:c03cffe402df | 621 | |
slowness | 0:c03cffe402df | 622 | #ifdef AX12_TRIGGER_DEBUG |
slowness | 0:c03cffe402df | 623 | printf(" ID : %d\n",TxBuf[2]); |
slowness | 0:c03cffe402df | 624 | #endif |
slowness | 0:c03cffe402df | 625 | |
slowness | 0:c03cffe402df | 626 | // Length |
slowness | 0:c03cffe402df | 627 | TxBuf[3] = 0x02; |
slowness | 0:c03cffe402df | 628 | sum += TxBuf[3]; |
slowness | 0:c03cffe402df | 629 | |
slowness | 0:c03cffe402df | 630 | #ifdef AX12_TRIGGER_DEBUG |
slowness | 0:c03cffe402df | 631 | printf(" Length %d\n",TxBuf[3]); |
slowness | 0:c03cffe402df | 632 | #endif |
slowness | 0:c03cffe402df | 633 | |
slowness | 0:c03cffe402df | 634 | // Instruction - ACTION |
slowness | 0:c03cffe402df | 635 | TxBuf[4] = 0x06; //reset |
slowness | 0:c03cffe402df | 636 | sum += TxBuf[4]; |
slowness | 0:c03cffe402df | 637 | |
slowness | 0:c03cffe402df | 638 | #ifdef AX12_TRIGGER_DEBUG |
slowness | 0:c03cffe402df | 639 | printf(" Instruction 0x%X\n",TxBuf[5]); |
slowness | 0:c03cffe402df | 640 | #endif |
slowness | 0:c03cffe402df | 641 | |
slowness | 0:c03cffe402df | 642 | // Checksum |
slowness | 0:c03cffe402df | 643 | TxBuf[5] = 0xFF - sum; |
slowness | 0:c03cffe402df | 644 | //#ifdef AX12_TRIGGER_DEBUG |
slowness | 0:c03cffe402df | 645 | printf(" Checksum 0x%X\n",TxBuf[5]); |
slowness | 0:c03cffe402df | 646 | //#endif |
slowness | 0:c03cffe402df | 647 | |
slowness | 0:c03cffe402df | 648 | // Transmit the packet in one burst with no pausing |
slowness | 0:c03cffe402df | 649 | for (int i = 0; i < 6 ; i++) |
slowness | 0:c03cffe402df | 650 | { |
slowness | 0:c03cffe402df | 651 | while(_ax12.writeable()==0); |
slowness | 0:c03cffe402df | 652 | _ax12.putc(TxBuf[i]); |
slowness | 0:c03cffe402df | 653 | |
slowness | 0:c03cffe402df | 654 | } |
slowness | 0:c03cffe402df | 655 | wait(0.001); |
slowness | 0:c03cffe402df | 656 | debut=t.read_ms(); |
slowness | 0:c03cffe402df | 657 | |
slowness | 0:c03cffe402df | 658 | do |
slowness | 0:c03cffe402df | 659 | { |
slowness | 0:c03cffe402df | 660 | if (_ax12.readable()==-1) // reception du premier Header ( 0xFF ) |
slowness | 0:c03cffe402df | 661 | printf("%02x",_ax12.getc()); |
slowness | 0:c03cffe402df | 662 | } |
slowness | 0:c03cffe402df | 663 | while((t.read_ms()-debut)<500); |
slowness | 0:c03cffe402df | 664 | |
slowness | 0:c03cffe402df | 665 | printf("\n"); |
slowness | 0:c03cffe402df | 666 | return; |
slowness | 0:c03cffe402df | 667 | } |
slowness | 0:c03cffe402df | 668 | |
slowness | 0:c03cffe402df | 669 | void AX12::read_all_info(unsigned char start, unsigned char longueur) |
slowness | 0:c03cffe402df | 670 | { |
slowness | 0:c03cffe402df | 671 | |
slowness | 0:c03cffe402df | 672 | unsigned char TxBuf[16]; |
slowness | 0:c03cffe402df | 673 | unsigned char sum = 0; |
slowness | 0:c03cffe402df | 674 | unsigned long debut=0; |
slowness | 0:c03cffe402df | 675 | |
slowness | 0:c03cffe402df | 676 | #ifdef AX12_TRIGGER_DEBUG |
slowness | 0:c03cffe402df | 677 | // Build the TxPacket first in RAM, then we'll send in one go |
slowness | 0:c03cffe402df | 678 | printf("\nreset\n"); |
slowness | 0:c03cffe402df | 679 | printf("\nreset Packet\n Header : 0xFF, 0xFF\n"); |
slowness | 0:c03cffe402df | 680 | #endif |
slowness | 0:c03cffe402df | 681 | |
slowness | 0:c03cffe402df | 682 | TxBuf[0] = 0xFF; |
slowness | 0:c03cffe402df | 683 | TxBuf[1] = 0xFF; |
slowness | 0:c03cffe402df | 684 | |
slowness | 0:c03cffe402df | 685 | // ID - Broadcast |
slowness | 0:c03cffe402df | 686 | TxBuf[2] =_ID; |
slowness | 0:c03cffe402df | 687 | sum += TxBuf[2]; |
slowness | 0:c03cffe402df | 688 | |
slowness | 0:c03cffe402df | 689 | #ifdef AX12_TRIGGER_DEBUG |
slowness | 0:c03cffe402df | 690 | printf(" ID : %d\n",TxBuf[2]); |
slowness | 0:c03cffe402df | 691 | #endif |
slowness | 0:c03cffe402df | 692 | |
slowness | 0:c03cffe402df | 693 | // Length |
slowness | 0:c03cffe402df | 694 | TxBuf[3] = 0x04; |
slowness | 0:c03cffe402df | 695 | sum += TxBuf[3]; |
slowness | 0:c03cffe402df | 696 | |
slowness | 0:c03cffe402df | 697 | #ifdef AX12_TRIGGER_DEBUG |
slowness | 0:c03cffe402df | 698 | printf(" Length %d\n",TxBuf[3]); |
slowness | 0:c03cffe402df | 699 | #endif |
slowness | 0:c03cffe402df | 700 | |
slowness | 0:c03cffe402df | 701 | // Instruction - ACTION |
slowness | 0:c03cffe402df | 702 | TxBuf[4] = INST_READ; //reset |
slowness | 0:c03cffe402df | 703 | sum += TxBuf[4]; |
slowness | 0:c03cffe402df | 704 | |
slowness | 0:c03cffe402df | 705 | #ifdef AX12_TRIGGER_DEBUG |
slowness | 0:c03cffe402df | 706 | printf(" Instruction 0x%X\n",TxBuf[5]); |
slowness | 0:c03cffe402df | 707 | #endif |
slowness | 0:c03cffe402df | 708 | |
slowness | 0:c03cffe402df | 709 | TxBuf[5] = start; //reset |
slowness | 0:c03cffe402df | 710 | sum += TxBuf[5]; |
slowness | 0:c03cffe402df | 711 | |
slowness | 0:c03cffe402df | 712 | TxBuf[6] = longueur; //reset |
slowness | 0:c03cffe402df | 713 | sum += TxBuf[6]; |
slowness | 0:c03cffe402df | 714 | |
slowness | 0:c03cffe402df | 715 | |
slowness | 0:c03cffe402df | 716 | // Checksum |
slowness | 0:c03cffe402df | 717 | TxBuf[7] = 0xFF - sum; |
slowness | 0:c03cffe402df | 718 | //#ifdef AX12_TRIGGER_DEBUG |
slowness | 0:c03cffe402df | 719 | //printf(" Checksum 0x%X\n\r",TxBuf[7]); |
slowness | 0:c03cffe402df | 720 | //#endif |
slowness | 0:c03cffe402df | 721 | |
slowness | 0:c03cffe402df | 722 | // Transmit the packet in one burst with no pausing |
slowness | 0:c03cffe402df | 723 | for (int i = 0; i < 8 ; i++) |
slowness | 0:c03cffe402df | 724 | { |
slowness | 0:c03cffe402df | 725 | while(_ax12.writeable()==0); |
slowness | 0:c03cffe402df | 726 | _ax12.putc(TxBuf[i]); |
slowness | 0:c03cffe402df | 727 | |
slowness | 0:c03cffe402df | 728 | } |
slowness | 0:c03cffe402df | 729 | |
slowness | 0:c03cffe402df | 730 | debut=t.read_ms(); |
slowness | 0:c03cffe402df | 731 | int i=0; |
slowness | 0:c03cffe402df | 732 | do |
slowness | 0:c03cffe402df | 733 | { |
slowness | 0:c03cffe402df | 734 | if (_ax12.readable()) |
slowness | 0:c03cffe402df | 735 | { // reception du premier Header ( 0xFF ) |
slowness | 0:c03cffe402df | 736 | printf("%02d:%02x ",start+i,_ax12.getc()); |
slowness | 0:c03cffe402df | 737 | i++; |
slowness | 0:c03cffe402df | 738 | } |
slowness | 0:c03cffe402df | 739 | } |
slowness | 0:c03cffe402df | 740 | while((t.read_ms()-debut)<5000); |
slowness | 0:c03cffe402df | 741 | |
slowness | 0:c03cffe402df | 742 | printf("\n"); |
slowness | 0:c03cffe402df | 743 | return; |
slowness | 0:c03cffe402df | 744 | } |
slowness | 0:c03cffe402df | 745 | |
slowness | 0:c03cffe402df | 746 | |
slowness | 0:c03cffe402df | 747 | void AX12::trigger(void) |
slowness | 0:c03cffe402df | 748 | { |
slowness | 0:c03cffe402df | 749 | |
slowness | 0:c03cffe402df | 750 | char TxBuf[16]; |
slowness | 0:c03cffe402df | 751 | char sum = 0; |
slowness | 0:c03cffe402df | 752 | |
slowness | 0:c03cffe402df | 753 | #ifdef AX12_TRIGGER_DEBUG |
slowness | 0:c03cffe402df | 754 | // Build the TxPacket first in RAM, then we'll send in one go |
slowness | 0:c03cffe402df | 755 | printf("\nTriggered\n"); |
slowness | 0:c03cffe402df | 756 | printf("\nTrigger Packet\n Header : 0xFF, 0xFF\n"); |
slowness | 0:c03cffe402df | 757 | #endif |
slowness | 0:c03cffe402df | 758 | |
slowness | 0:c03cffe402df | 759 | TxBuf[0] = 0xFF; |
slowness | 0:c03cffe402df | 760 | TxBuf[1] = 0xFF; |
slowness | 0:c03cffe402df | 761 | |
slowness | 0:c03cffe402df | 762 | // ID - Broadcast |
slowness | 0:c03cffe402df | 763 | TxBuf[2] = 0xFE; |
slowness | 0:c03cffe402df | 764 | sum += TxBuf[2]; |
slowness | 0:c03cffe402df | 765 | |
slowness | 0:c03cffe402df | 766 | #ifdef AX12_TRIGGER_DEBUG |
slowness | 0:c03cffe402df | 767 | printf(" ID : %d\n",TxBuf[2]); |
slowness | 0:c03cffe402df | 768 | #endif |
slowness | 0:c03cffe402df | 769 | |
slowness | 0:c03cffe402df | 770 | // Length |
slowness | 0:c03cffe402df | 771 | TxBuf[3] = 0x02; |
slowness | 0:c03cffe402df | 772 | sum += TxBuf[3]; |
slowness | 0:c03cffe402df | 773 | |
slowness | 0:c03cffe402df | 774 | #ifdef AX12_TRIGGER_DEBUG |
slowness | 0:c03cffe402df | 775 | printf(" Length %d\n",TxBuf[3]); |
slowness | 0:c03cffe402df | 776 | #endif |
slowness | 0:c03cffe402df | 777 | |
slowness | 0:c03cffe402df | 778 | // Instruction - ACTION |
slowness | 0:c03cffe402df | 779 | TxBuf[4] = 0x04; |
slowness | 0:c03cffe402df | 780 | sum += TxBuf[4]; |
slowness | 0:c03cffe402df | 781 | |
slowness | 0:c03cffe402df | 782 | #ifdef AX12_TRIGGER_DEBUG |
slowness | 0:c03cffe402df | 783 | printf(" Instruction 0x%X\n",TxBuf[5]); |
slowness | 0:c03cffe402df | 784 | #endif |
slowness | 0:c03cffe402df | 785 | |
slowness | 0:c03cffe402df | 786 | // Checksum |
slowness | 0:c03cffe402df | 787 | TxBuf[5] = 0xFF - sum; |
slowness | 0:c03cffe402df | 788 | #ifdef AX12_TRIGGER_DEBUG |
slowness | 0:c03cffe402df | 789 | printf(" Checksum 0x%X\n",TxBuf[5]); |
slowness | 0:c03cffe402df | 790 | #endif |
slowness | 0:c03cffe402df | 791 | |
slowness | 0:c03cffe402df | 792 | // Transmit the packet in one burst with no pausing |
slowness | 0:c03cffe402df | 793 | for (int i = 0; i < 6 ; i++) { |
slowness | 0:c03cffe402df | 794 | _ax12.putc(TxBuf[i]); |
slowness | 0:c03cffe402df | 795 | } |
slowness | 0:c03cffe402df | 796 | |
slowness | 0:c03cffe402df | 797 | // This is a broadcast packet, so there will be no reply |
slowness | 0:c03cffe402df | 798 | return; |
slowness | 0:c03cffe402df | 799 | } |
slowness | 0:c03cffe402df | 800 | |
slowness | 0:c03cffe402df | 801 | |
slowness | 0:c03cffe402df | 802 | float AX12::Get_Position(void) |
slowness | 0:c03cffe402df | 803 | { |
slowness | 0:c03cffe402df | 804 | |
slowness | 0:c03cffe402df | 805 | #ifdef AX12_DEBUG |
slowness | 0:c03cffe402df | 806 | printf("\nGetPosition(%d)",_ID); |
slowness | 0:c03cffe402df | 807 | #endif |
slowness | 0:c03cffe402df | 808 | |
slowness | 0:c03cffe402df | 809 | char data[2]; |
slowness | 0:c03cffe402df | 810 | |
slowness | 0:c03cffe402df | 811 | int ErrorCode = read(_ID, AX12_REG_POSITION, 2, data); |
slowness | 0:c03cffe402df | 812 | int position = data[0] | (data[1] << 8); |
slowness | 0:c03cffe402df | 813 | float angle = ((float)position * 300.0)/1023.0; |
slowness | 0:c03cffe402df | 814 | |
slowness | 0:c03cffe402df | 815 | return (angle); |
slowness | 0:c03cffe402df | 816 | } |
slowness | 0:c03cffe402df | 817 | |
slowness | 0:c03cffe402df | 818 | |
slowness | 0:c03cffe402df | 819 | float AX12::Get_Temp (void) |
slowness | 0:c03cffe402df | 820 | { |
slowness | 0:c03cffe402df | 821 | |
slowness | 0:c03cffe402df | 822 | #ifdef AX12_DEBUG |
slowness | 0:c03cffe402df | 823 | printf("\nGetTemp(%d)",_ID); |
slowness | 0:c03cffe402df | 824 | #endif |
slowness | 0:c03cffe402df | 825 | |
slowness | 0:c03cffe402df | 826 | char data[1]; |
slowness | 0:c03cffe402df | 827 | int ErrorCode = read(_ID, AX12_REG_TEMP, 1, data); |
slowness | 0:c03cffe402df | 828 | float temp = data[0]; |
slowness | 0:c03cffe402df | 829 | return(temp); |
slowness | 0:c03cffe402df | 830 | } |
slowness | 0:c03cffe402df | 831 | |
slowness | 0:c03cffe402df | 832 | |
slowness | 0:c03cffe402df | 833 | float AX12::Get_Volts (void) |
slowness | 0:c03cffe402df | 834 | { |
slowness | 0:c03cffe402df | 835 | |
slowness | 0:c03cffe402df | 836 | #ifdef AX12_DEBUG |
slowness | 0:c03cffe402df | 837 | printf("\nGetVolts(%d)",_ID); |
slowness | 0:c03cffe402df | 838 | #endif |
slowness | 0:c03cffe402df | 839 | |
slowness | 0:c03cffe402df | 840 | char data[1]; |
slowness | 0:c03cffe402df | 841 | int ErrorCode = read(_ID, AX12_REG_VOLTS, 1, data); |
slowness | 0:c03cffe402df | 842 | float volts = data[0]/10.0; |
slowness | 0:c03cffe402df | 843 | return(volts); |
slowness | 0:c03cffe402df | 844 | } |
slowness | 0:c03cffe402df | 845 | |
slowness | 0:c03cffe402df | 846 | |
slowness | 0:c03cffe402df | 847 | int AX12::read(int ID, int start, int bytes, char* data) { |
slowness | 0:c03cffe402df | 848 | |
slowness | 0:c03cffe402df | 849 | |
slowness | 0:c03cffe402df | 850 | char PacketLength = 0x3; |
slowness | 0:c03cffe402df | 851 | char TxBuf[16]; |
slowness | 0:c03cffe402df | 852 | char sum = 0; |
slowness | 0:c03cffe402df | 853 | char Status[16]; |
slowness | 0:c03cffe402df | 854 | |
slowness | 0:c03cffe402df | 855 | int timeout = 0; |
slowness | 0:c03cffe402df | 856 | int plen = 0; |
slowness | 0:c03cffe402df | 857 | int flag_out = 0; |
slowness | 0:c03cffe402df | 858 | int timeout_transmit = 0; |
slowness | 0:c03cffe402df | 859 | int i = 0; |
slowness | 0:c03cffe402df | 860 | int enable = 0; |
slowness | 0:c03cffe402df | 861 | // int poubelle = 0; |
slowness | 0:c03cffe402df | 862 | // int count = 0; |
slowness | 0:c03cffe402df | 863 | // char vidage[50]; |
slowness | 0:c03cffe402df | 864 | |
slowness | 0:c03cffe402df | 865 | typedef enum {Header1, Header2, ident, length, erreur, reception, checksum} type_etat; |
slowness | 0:c03cffe402df | 866 | type_etat etat = Header1; |
slowness | 0:c03cffe402df | 867 | |
slowness | 0:c03cffe402df | 868 | Status[4] = 0xFE; // return code |
slowness | 0:c03cffe402df | 869 | |
slowness | 0:c03cffe402df | 870 | |
slowness | 0:c03cffe402df | 871 | |
slowness | 0:c03cffe402df | 872 | |
slowness | 0:c03cffe402df | 873 | |
slowness | 0:c03cffe402df | 874 | /*********************************** CREATION DE LA TRAME A EVOYER *****************************************/ |
slowness | 0:c03cffe402df | 875 | |
slowness | 0:c03cffe402df | 876 | #ifdef AX12_READ_DEBUG |
slowness | 0:c03cffe402df | 877 | printf("\nread(%d,0x%x,%d,data)\n",ID,start,bytes); |
slowness | 0:c03cffe402df | 878 | #endif |
slowness | 0:c03cffe402df | 879 | |
slowness | 0:c03cffe402df | 880 | // Build the TxPacket first in RAM, then we'll send in one go |
slowness | 0:c03cffe402df | 881 | #ifdef AX12_READ_DEBUG |
slowness | 0:c03cffe402df | 882 | printf("\nInstruction Packet\n Header : 0xFF, 0xFF\n"); |
slowness | 0:c03cffe402df | 883 | #endif |
slowness | 0:c03cffe402df | 884 | |
slowness | 0:c03cffe402df | 885 | TxBuf[0] = 0xff; |
slowness | 0:c03cffe402df | 886 | TxBuf[1] = 0xff; |
slowness | 0:c03cffe402df | 887 | |
slowness | 0:c03cffe402df | 888 | // ID |
slowness | 0:c03cffe402df | 889 | TxBuf[2] = ID; |
slowness | 0:c03cffe402df | 890 | sum += TxBuf[2]; |
slowness | 0:c03cffe402df | 891 | |
slowness | 0:c03cffe402df | 892 | #ifdef AX12_READ_DEBUG |
slowness | 0:c03cffe402df | 893 | printf(" ID : %d\n",TxBuf[2]); |
slowness | 0:c03cffe402df | 894 | #endif |
slowness | 0:c03cffe402df | 895 | |
slowness | 0:c03cffe402df | 896 | // Packet Length |
slowness | 0:c03cffe402df | 897 | TxBuf[3] = PacketLength+bytes; // Length = 4 ; 2 + 1 (start) = 1 (bytes) |
slowness | 0:c03cffe402df | 898 | sum += TxBuf[3]; // Accululate the packet sum |
slowness | 0:c03cffe402df | 899 | |
slowness | 0:c03cffe402df | 900 | #ifdef AX12_READ_DEBUG |
slowness | 0:c03cffe402df | 901 | printf(" Length : 0x%x\n",TxBuf[3]); |
slowness | 0:c03cffe402df | 902 | #endif |
slowness | 0:c03cffe402df | 903 | |
slowness | 0:c03cffe402df | 904 | // Instruction - Read |
slowness | 0:c03cffe402df | 905 | TxBuf[4] = 0x2; |
slowness | 0:c03cffe402df | 906 | sum += TxBuf[4]; |
slowness | 0:c03cffe402df | 907 | |
slowness | 0:c03cffe402df | 908 | #ifdef AX12_READ_DEBUG |
slowness | 0:c03cffe402df | 909 | printf(" Instruction : 0x%x\n",TxBuf[4]); |
slowness | 0:c03cffe402df | 910 | #endif |
slowness | 0:c03cffe402df | 911 | |
slowness | 0:c03cffe402df | 912 | // Start Address |
slowness | 0:c03cffe402df | 913 | TxBuf[5] = start; |
slowness | 0:c03cffe402df | 914 | sum += TxBuf[5]; |
slowness | 0:c03cffe402df | 915 | |
slowness | 0:c03cffe402df | 916 | #ifdef AX12_READ_DEBUG |
slowness | 0:c03cffe402df | 917 | printf(" Start Address : 0x%x\n",TxBuf[5]); |
slowness | 0:c03cffe402df | 918 | #endif |
slowness | 0:c03cffe402df | 919 | |
slowness | 0:c03cffe402df | 920 | // Bytes to read |
slowness | 0:c03cffe402df | 921 | TxBuf[6] = bytes; |
slowness | 0:c03cffe402df | 922 | sum += TxBuf[6]; |
slowness | 0:c03cffe402df | 923 | |
slowness | 0:c03cffe402df | 924 | #ifdef AX12_READ_DEBUG |
slowness | 0:c03cffe402df | 925 | printf(" No bytes : 0x%x\n",TxBuf[6]); |
slowness | 0:c03cffe402df | 926 | #endif |
slowness | 0:c03cffe402df | 927 | |
slowness | 0:c03cffe402df | 928 | // Checksum |
slowness | 0:c03cffe402df | 929 | TxBuf[7] = 0xFF - sum; |
slowness | 0:c03cffe402df | 930 | #ifdef AX12_READ_DEBUG |
slowness | 0:c03cffe402df | 931 | printf(" Checksum : 0x%x\n",TxBuf[7]); |
slowness | 0:c03cffe402df | 932 | #endif |
slowness | 0:c03cffe402df | 933 | /********************************************TRAME CONSTRUITE DANS TxBuf***************************************/ |
slowness | 0:c03cffe402df | 934 | |
slowness | 0:c03cffe402df | 935 | |
slowness | 0:c03cffe402df | 936 | |
slowness | 0:c03cffe402df | 937 | |
slowness | 0:c03cffe402df | 938 | /* Transmission de la trame construite precedemment dans le tableau TxBuf |
slowness | 0:c03cffe402df | 939 | */ |
slowness | 0:c03cffe402df | 940 | while ((timeout_transmit<1000) && (i < (7+bytes))) |
slowness | 0:c03cffe402df | 941 | { |
slowness | 0:c03cffe402df | 942 | if (_ax12.writeable()) |
slowness | 0:c03cffe402df | 943 | { |
slowness | 0:c03cffe402df | 944 | _ax12.putc(TxBuf[i]); |
slowness | 0:c03cffe402df | 945 | i++; |
slowness | 0:c03cffe402df | 946 | timeout_transmit = 0; |
slowness | 0:c03cffe402df | 947 | } |
slowness | 0:c03cffe402df | 948 | else timeout_transmit++; |
slowness | 0:c03cffe402df | 949 | } |
slowness | 0:c03cffe402df | 950 | |
slowness | 0:c03cffe402df | 951 | if (timeout_transmit == 1000 ) // dans le cas d'une sortie en timeout pour ne pas rester bloquer ! |
slowness | 0:c03cffe402df | 952 | { |
slowness | 0:c03cffe402df | 953 | #ifdef AX12_DEBUG |
slowness | 0:c03cffe402df | 954 | printf ("timeout transmit erreur\r\n"); |
slowness | 0:c03cffe402df | 955 | #endif |
slowness | 0:c03cffe402df | 956 | return(-1); |
slowness | 0:c03cffe402df | 957 | } |
slowness | 0:c03cffe402df | 958 | /* Transmission effectuée on va ensuite récuperer la trame de retour renvoyer par le servomoteur |
slowness | 0:c03cffe402df | 959 | */ |
slowness | 0:c03cffe402df | 960 | |
slowness | 0:c03cffe402df | 961 | |
slowness | 0:c03cffe402df | 962 | // Wait for the bytes to be transmitted |
slowness | 0:c03cffe402df | 963 | wait (0.001); |
slowness | 0:c03cffe402df | 964 | |
slowness | 0:c03cffe402df | 965 | |
slowness | 0:c03cffe402df | 966 | |
slowness | 0:c03cffe402df | 967 | // Skip if the read was to the broadcast address |
slowness | 0:c03cffe402df | 968 | if (_ID != 0xFE) { |
slowness | 0:c03cffe402df | 969 | |
slowness | 0:c03cffe402df | 970 | |
slowness | 0:c03cffe402df | 971 | |
slowness | 0:c03cffe402df | 972 | /* Partie de reception de la trame de retour |
slowness | 0:c03cffe402df | 973 | */ |
slowness | 0:c03cffe402df | 974 | while ((flag_out != 1) && (timeout < (1000*bytes))) |
slowness | 0:c03cffe402df | 975 | { |
slowness | 0:c03cffe402df | 976 | // Les differents etats de l'automate on été créés au debut de la fonction write ! |
slowness | 0:c03cffe402df | 977 | switch (etat) |
slowness | 0:c03cffe402df | 978 | { |
slowness | 0:c03cffe402df | 979 | case Header1: if (_ax12.readable()) // reception du premier Header ( 0xFF ) |
slowness | 0:c03cffe402df | 980 | { |
slowness | 0:c03cffe402df | 981 | Status[plen] = _ax12.getc(); |
slowness | 0:c03cffe402df | 982 | timeout = 0; |
slowness | 0:c03cffe402df | 983 | if (Status[plen] == 0xFF ) |
slowness | 0:c03cffe402df | 984 | { |
slowness | 0:c03cffe402df | 985 | etat = Header2; |
slowness | 0:c03cffe402df | 986 | #ifdef AX12_DEBUG_READ |
slowness | 0:c03cffe402df | 987 | printf("data[%d] : %d\r\n", plen, (int)Status[plen]); |
slowness | 0:c03cffe402df | 988 | #endif |
slowness | 0:c03cffe402df | 989 | plen++; |
slowness | 0:c03cffe402df | 990 | |
slowness | 0:c03cffe402df | 991 | } |
slowness | 0:c03cffe402df | 992 | else etat = Header1; |
slowness | 0:c03cffe402df | 993 | } |
slowness | 0:c03cffe402df | 994 | else timeout++; |
slowness | 0:c03cffe402df | 995 | break; |
slowness | 0:c03cffe402df | 996 | |
slowness | 0:c03cffe402df | 997 | |
slowness | 0:c03cffe402df | 998 | case Header2: if (_ax12.readable()) // reception du second Header ( 0xFF ) |
slowness | 0:c03cffe402df | 999 | { |
slowness | 0:c03cffe402df | 1000 | Status[plen] = _ax12.getc(); |
slowness | 0:c03cffe402df | 1001 | timeout = 0; |
slowness | 0:c03cffe402df | 1002 | if (Status[plen] == 0xFF ) |
slowness | 0:c03cffe402df | 1003 | { |
slowness | 0:c03cffe402df | 1004 | etat = ident; |
slowness | 0:c03cffe402df | 1005 | #ifdef AX12_DEBUG_READ |
slowness | 0:c03cffe402df | 1006 | printf("data[%d] : %d\r\n", plen, (int)Status[plen]); |
slowness | 0:c03cffe402df | 1007 | #endif |
slowness | 0:c03cffe402df | 1008 | plen++; |
slowness | 0:c03cffe402df | 1009 | |
slowness | 0:c03cffe402df | 1010 | } |
slowness | 0:c03cffe402df | 1011 | else if (Status[plen] == ID ) // PERMET D'EVITER CERTAINES ERREUR LORSQU'ON LIT PLUSIEURS REGISTRES !!!! |
slowness | 0:c03cffe402df | 1012 | { |
slowness | 0:c03cffe402df | 1013 | Status[plen] = 0; |
slowness | 0:c03cffe402df | 1014 | #ifdef AX12_DEBUG_READ |
slowness | 0:c03cffe402df | 1015 | printf("data[%d] : %d\r\n", plen, (int)Status[plen]); |
slowness | 0:c03cffe402df | 1016 | #endif |
slowness | 0:c03cffe402df | 1017 | plen++; |
slowness | 0:c03cffe402df | 1018 | Status[plen] = ID; |
slowness | 0:c03cffe402df | 1019 | etat = length; |
slowness | 0:c03cffe402df | 1020 | #ifdef AX12_DEBUG_READ |
slowness | 0:c03cffe402df | 1021 | printf("data[%d] : %d\r\n", plen, (int)Status[plen]); |
slowness | 0:c03cffe402df | 1022 | #endif |
slowness | 0:c03cffe402df | 1023 | plen++; |
slowness | 0:c03cffe402df | 1024 | |
slowness | 0:c03cffe402df | 1025 | } |
slowness | 0:c03cffe402df | 1026 | else |
slowness | 0:c03cffe402df | 1027 | { |
slowness | 0:c03cffe402df | 1028 | |
slowness | 0:c03cffe402df | 1029 | etat = Header1; |
slowness | 0:c03cffe402df | 1030 | plen = 0; |
slowness | 0:c03cffe402df | 1031 | } |
slowness | 0:c03cffe402df | 1032 | } |
slowness | 0:c03cffe402df | 1033 | else timeout++; |
slowness | 0:c03cffe402df | 1034 | break; |
slowness | 0:c03cffe402df | 1035 | |
slowness | 0:c03cffe402df | 1036 | case ident: if (_ax12.readable()) // reception de l'octet correspondant à l'ID du servomoteur |
slowness | 0:c03cffe402df | 1037 | { |
slowness | 0:c03cffe402df | 1038 | Status[plen] = _ax12.getc(); |
slowness | 0:c03cffe402df | 1039 | timeout = 0; |
slowness | 0:c03cffe402df | 1040 | if (Status[plen] == ID ) |
slowness | 0:c03cffe402df | 1041 | { |
slowness | 0:c03cffe402df | 1042 | etat = length; |
slowness | 0:c03cffe402df | 1043 | #ifdef AX12_DEBUG_READ |
slowness | 0:c03cffe402df | 1044 | printf("data[%d] : %d\r\n", plen, (int)Status[plen]); |
slowness | 0:c03cffe402df | 1045 | #endif |
slowness | 0:c03cffe402df | 1046 | plen++; |
slowness | 0:c03cffe402df | 1047 | |
slowness | 0:c03cffe402df | 1048 | } |
slowness | 0:c03cffe402df | 1049 | else |
slowness | 0:c03cffe402df | 1050 | { |
slowness | 0:c03cffe402df | 1051 | etat = Header1; |
slowness | 0:c03cffe402df | 1052 | plen = 0; |
slowness | 0:c03cffe402df | 1053 | } |
slowness | 0:c03cffe402df | 1054 | } |
slowness | 0:c03cffe402df | 1055 | else timeout++; |
slowness | 0:c03cffe402df | 1056 | break; |
slowness | 0:c03cffe402df | 1057 | |
slowness | 0:c03cffe402df | 1058 | case length: if (_ax12.readable()) // reception de l'octet correspondant à la taille ( taille = 2 + nombre de paramètres ) |
slowness | 0:c03cffe402df | 1059 | { |
slowness | 0:c03cffe402df | 1060 | Status[plen] = _ax12.getc(); |
slowness | 0:c03cffe402df | 1061 | timeout = 0; |
slowness | 0:c03cffe402df | 1062 | if (Status[plen] == (bytes+2) ) |
slowness | 0:c03cffe402df | 1063 | { |
slowness | 0:c03cffe402df | 1064 | etat = erreur; |
slowness | 0:c03cffe402df | 1065 | #ifdef AX12_DEBUG_READ |
slowness | 0:c03cffe402df | 1066 | printf("data[%d] : %d\r\n", plen, (int)Status[plen]); |
slowness | 0:c03cffe402df | 1067 | #endif |
slowness | 0:c03cffe402df | 1068 | plen++; |
slowness | 0:c03cffe402df | 1069 | |
slowness | 0:c03cffe402df | 1070 | } |
slowness | 0:c03cffe402df | 1071 | else |
slowness | 0:c03cffe402df | 1072 | { |
slowness | 0:c03cffe402df | 1073 | etat = Header1; |
slowness | 0:c03cffe402df | 1074 | plen = 0; |
slowness | 0:c03cffe402df | 1075 | } |
slowness | 0:c03cffe402df | 1076 | } |
slowness | 0:c03cffe402df | 1077 | else timeout++; |
slowness | 0:c03cffe402df | 1078 | break; |
slowness | 0:c03cffe402df | 1079 | |
slowness | 0:c03cffe402df | 1080 | case erreur: if (_ax12.readable()) //reception de l'octet correspondant au code d'erreurs eventuels ( 0 = pas d'erreur ) |
slowness | 0:c03cffe402df | 1081 | { |
slowness | 0:c03cffe402df | 1082 | Status[plen] = _ax12.getc(); |
slowness | 0:c03cffe402df | 1083 | timeout = 0; |
slowness | 0:c03cffe402df | 1084 | #ifdef AX12_DEBUG_READ |
slowness | 0:c03cffe402df | 1085 | printf("data[%d] : %d\r\n", plen, (int)Status[plen]); |
slowness | 0:c03cffe402df | 1086 | #endif |
slowness | 0:c03cffe402df | 1087 | plen++; |
slowness | 0:c03cffe402df | 1088 | |
slowness | 0:c03cffe402df | 1089 | etat = reception; |
slowness | 0:c03cffe402df | 1090 | } |
slowness | 0:c03cffe402df | 1091 | else timeout++; |
slowness | 0:c03cffe402df | 1092 | |
slowness | 0:c03cffe402df | 1093 | case reception: while ( enable < bytes ) // reception du ou des octect(s) de donnés ( suivant la valeur de la variable length ) |
slowness | 0:c03cffe402df | 1094 | { |
slowness | 0:c03cffe402df | 1095 | if (_ax12.readable()) |
slowness | 0:c03cffe402df | 1096 | { |
slowness | 0:c03cffe402df | 1097 | Status[plen] = _ax12.getc(); |
slowness | 0:c03cffe402df | 1098 | timeout = 0; |
slowness | 0:c03cffe402df | 1099 | #ifdef AX12_DEBUG_READ |
slowness | 0:c03cffe402df | 1100 | printf("data[%d] : %d\r\n", plen, (int)Status[plen]); |
slowness | 0:c03cffe402df | 1101 | #endif |
slowness | 0:c03cffe402df | 1102 | plen++; |
slowness | 0:c03cffe402df | 1103 | enable++; |
slowness | 0:c03cffe402df | 1104 | |
slowness | 0:c03cffe402df | 1105 | } |
slowness | 0:c03cffe402df | 1106 | else timeout++; |
slowness | 0:c03cffe402df | 1107 | } |
slowness | 0:c03cffe402df | 1108 | etat = checksum; |
slowness | 0:c03cffe402df | 1109 | break; |
slowness | 0:c03cffe402df | 1110 | |
slowness | 0:c03cffe402df | 1111 | 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 | 1112 | { |
slowness | 0:c03cffe402df | 1113 | Status[plen] = _ax12.getc(); |
slowness | 0:c03cffe402df | 1114 | timeout = 0; |
slowness | 0:c03cffe402df | 1115 | flag_out = 1; |
slowness | 0:c03cffe402df | 1116 | etat = Header1; |
slowness | 0:c03cffe402df | 1117 | |
slowness | 0:c03cffe402df | 1118 | #ifdef AX12_DEBUG_READ |
slowness | 0:c03cffe402df | 1119 | printf("data[%d] : %d\r\n\n", plen, (int)Status[plen]); |
slowness | 0:c03cffe402df | 1120 | #endif |
slowness | 0:c03cffe402df | 1121 | } |
slowness | 0:c03cffe402df | 1122 | else timeout++; |
slowness | 0:c03cffe402df | 1123 | break; |
slowness | 0:c03cffe402df | 1124 | |
slowness | 0:c03cffe402df | 1125 | default: break; |
slowness | 0:c03cffe402df | 1126 | } |
slowness | 0:c03cffe402df | 1127 | } |
slowness | 0:c03cffe402df | 1128 | |
slowness | 0:c03cffe402df | 1129 | |
slowness | 0:c03cffe402df | 1130 | 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 | 1131 | { |
slowness | 0:c03cffe402df | 1132 | #ifdef AX12_DEBUG |
slowness | 0:c03cffe402df | 1133 | printf ("timeout erreur\n"); |
slowness | 0:c03cffe402df | 1134 | #endif |
slowness | 0:c03cffe402df | 1135 | return(-1); |
slowness | 0:c03cffe402df | 1136 | } |
slowness | 0:c03cffe402df | 1137 | |
slowness | 0:c03cffe402df | 1138 | |
slowness | 0:c03cffe402df | 1139 | // copie des données dans le tableau data |
slowness | 0:c03cffe402df | 1140 | for (int i=0; i < Status[3]-2 ; i++) |
slowness | 0:c03cffe402df | 1141 | { |
slowness | 0:c03cffe402df | 1142 | data[i] = Status[5+i]; |
slowness | 0:c03cffe402df | 1143 | } |
slowness | 0:c03cffe402df | 1144 | |
slowness | 0:c03cffe402df | 1145 | } // toute la partie precedente ne s'effectue pas dans le cas d'un appel avec un broadcast ID (ID!=0xFE) |
slowness | 0:c03cffe402df | 1146 | |
slowness | 0:c03cffe402df | 1147 | return(Status[4]); // retourne le code d'erreur ( octect 5 de la trame de retour ) |
slowness | 0:c03cffe402df | 1148 | } |
slowness | 0:c03cffe402df | 1149 | |
slowness | 0:c03cffe402df | 1150 | int AX12::write(int ID, int start, int bytes, char* data, int flag) |
slowness | 0:c03cffe402df | 1151 | { |
slowness | 0:c03cffe402df | 1152 | // 0xff, 0xff, ID, Length, Intruction(write), Address, Param(s), Checksum |
slowness | 0:c03cffe402df | 1153 | |
slowness | 0:c03cffe402df | 1154 | char TxBuf[16]; |
slowness | 0:c03cffe402df | 1155 | char sum = 0; |
slowness | 0:c03cffe402df | 1156 | char Status[6]; |
slowness | 0:c03cffe402df | 1157 | |
slowness | 0:c03cffe402df | 1158 | int timeout = 0; |
slowness | 0:c03cffe402df | 1159 | int plen = 0; |
slowness | 0:c03cffe402df | 1160 | int flag_out = 0; |
slowness | 0:c03cffe402df | 1161 | int timeout_transmit = 0; |
slowness | 0:c03cffe402df | 1162 | int i = 0; |
slowness | 0:c03cffe402df | 1163 | int poubelle = 0; |
slowness | 0:c03cffe402df | 1164 | int count = 0; |
slowness | 0:c03cffe402df | 1165 | char vidage[50]; |
slowness | 0:c03cffe402df | 1166 | |
slowness | 0:c03cffe402df | 1167 | typedef enum {Header1, Header2, ident, length, erreur, checksum} type_etat; |
slowness | 0:c03cffe402df | 1168 | type_etat etat = Header1; |
slowness | 0:c03cffe402df | 1169 | |
slowness | 0:c03cffe402df | 1170 | #ifdef AX12_WRITE_DEBUG |
slowness | 0:c03cffe402df | 1171 | printf("\nwrite(%d,0x%x,%d,data,%d)\n",ID,start,bytes,flag); |
slowness | 0:c03cffe402df | 1172 | #endif |
slowness | 0:c03cffe402df | 1173 | |
slowness | 0:c03cffe402df | 1174 | // Build the TxPacket first in RAM, then we'll send in one go |
slowness | 0:c03cffe402df | 1175 | #ifdef AX12_WRITE_DEBUG |
slowness | 0:c03cffe402df | 1176 | printf("\nInstruction Packet\n Header : 0xFF, 0xFF\n"); |
slowness | 0:c03cffe402df | 1177 | #endif |
slowness | 0:c03cffe402df | 1178 | |
slowness | 0:c03cffe402df | 1179 | TxBuf[0] = 0xff; |
slowness | 0:c03cffe402df | 1180 | TxBuf[1] = 0xff; |
slowness | 0:c03cffe402df | 1181 | |
slowness | 0:c03cffe402df | 1182 | // ID |
slowness | 0:c03cffe402df | 1183 | TxBuf[2] = ID; |
slowness | 0:c03cffe402df | 1184 | sum += TxBuf[2]; |
slowness | 0:c03cffe402df | 1185 | |
slowness | 0:c03cffe402df | 1186 | #ifdef AX12_WRITE_DEBUG |
slowness | 0:c03cffe402df | 1187 | printf(" ID : %d\n",TxBuf[2]); |
slowness | 0:c03cffe402df | 1188 | #endif |
slowness | 0:c03cffe402df | 1189 | |
slowness | 0:c03cffe402df | 1190 | // packet Length |
slowness | 0:c03cffe402df | 1191 | TxBuf[3] = 3+bytes; |
slowness | 0:c03cffe402df | 1192 | sum += TxBuf[3]; |
slowness | 0:c03cffe402df | 1193 | |
slowness | 0:c03cffe402df | 1194 | #ifdef AX12_WRITE_DEBUG |
slowness | 0:c03cffe402df | 1195 | printf(" Length : %d\n",TxBuf[3]); |
slowness | 0:c03cffe402df | 1196 | #endif |
slowness | 0:c03cffe402df | 1197 | |
slowness | 0:c03cffe402df | 1198 | // Instruction |
slowness | 0:c03cffe402df | 1199 | if (flag == 1) { |
slowness | 0:c03cffe402df | 1200 | TxBuf[4]=0x04; |
slowness | 0:c03cffe402df | 1201 | sum += TxBuf[4]; |
slowness | 0:c03cffe402df | 1202 | } else { |
slowness | 0:c03cffe402df | 1203 | TxBuf[4]=0x03; |
slowness | 0:c03cffe402df | 1204 | sum += TxBuf[4]; |
slowness | 0:c03cffe402df | 1205 | } |
slowness | 0:c03cffe402df | 1206 | |
slowness | 0:c03cffe402df | 1207 | #ifdef AX12_WRITE_DEBUG |
slowness | 0:c03cffe402df | 1208 | printf(" Instruction : 0x%x\n",TxBuf[4]); |
slowness | 0:c03cffe402df | 1209 | #endif |
slowness | 0:c03cffe402df | 1210 | |
slowness | 0:c03cffe402df | 1211 | // Start Address |
slowness | 0:c03cffe402df | 1212 | TxBuf[5] = start; |
slowness | 0:c03cffe402df | 1213 | sum += TxBuf[5]; |
slowness | 0:c03cffe402df | 1214 | |
slowness | 0:c03cffe402df | 1215 | #ifdef AX12_WRITE_DEBUG |
slowness | 0:c03cffe402df | 1216 | printf(" Start : 0x%x\n",TxBuf[5]); |
slowness | 0:c03cffe402df | 1217 | #endif |
slowness | 0:c03cffe402df | 1218 | |
slowness | 0:c03cffe402df | 1219 | // data |
slowness | 0:c03cffe402df | 1220 | for (char i=0; i<bytes ; i++) { |
slowness | 0:c03cffe402df | 1221 | TxBuf[6+i] = data[i]; |
slowness | 0:c03cffe402df | 1222 | sum += TxBuf[6+i]; |
slowness | 0:c03cffe402df | 1223 | |
slowness | 0:c03cffe402df | 1224 | #ifdef AX12_WRITE_DEBUG |
slowness | 0:c03cffe402df | 1225 | printf(" Data : 0x%x\n",TxBuf[6+i]); |
slowness | 0:c03cffe402df | 1226 | #endif |
slowness | 0:c03cffe402df | 1227 | |
slowness | 0:c03cffe402df | 1228 | } |
slowness | 0:c03cffe402df | 1229 | |
slowness | 0:c03cffe402df | 1230 | // checksum |
slowness | 0:c03cffe402df | 1231 | TxBuf[6+bytes] = 0xFF - sum; |
slowness | 0:c03cffe402df | 1232 | |
slowness | 0:c03cffe402df | 1233 | #ifdef AX12_WRITE_DEBUG |
slowness | 0:c03cffe402df | 1234 | printf(" Checksum : 0x%x\n",TxBuf[6+bytes]); |
slowness | 0:c03cffe402df | 1235 | #endif |
slowness | 0:c03cffe402df | 1236 | |
slowness | 0:c03cffe402df | 1237 | |
slowness | 0:c03cffe402df | 1238 | /* Transmission de la trame construite precedemment dans le tableau TxBuf |
slowness | 0:c03cffe402df | 1239 | */ |
slowness | 0:c03cffe402df | 1240 | while ((timeout_transmit<100) && (i < (7+bytes))) |
slowness | 0:c03cffe402df | 1241 | { |
slowness | 0:c03cffe402df | 1242 | if (_ax12.writeable()) |
slowness | 0:c03cffe402df | 1243 | { |
slowness | 0:c03cffe402df | 1244 | _ax12.putc(TxBuf[i]); |
slowness | 0:c03cffe402df | 1245 | i++; |
slowness | 0:c03cffe402df | 1246 | timeout_transmit = 0; |
slowness | 0:c03cffe402df | 1247 | } |
slowness | 0:c03cffe402df | 1248 | else timeout_transmit++; |
slowness | 0:c03cffe402df | 1249 | } |
slowness | 0:c03cffe402df | 1250 | |
slowness | 0:c03cffe402df | 1251 | if (timeout_transmit == 100 ) // dans le cas d'une sortie en timeout pour ne pas rester bloquer ! |
slowness | 0:c03cffe402df | 1252 | { |
slowness | 0:c03cffe402df | 1253 | #ifdef AX12_DEBUG |
slowness | 0:c03cffe402df | 1254 | printf ("TIMEOUT TRANSMIT ERROR\r\n"); |
slowness | 0:c03cffe402df | 1255 | #endif |
slowness | 0:c03cffe402df | 1256 | return(-1); |
slowness | 0:c03cffe402df | 1257 | } |
slowness | 0:c03cffe402df | 1258 | /* Transmission effectuée on va ensuite récuperer la trame de retour renvoyer par le servomoteur |
slowness | 0:c03cffe402df | 1259 | */ |
slowness | 0:c03cffe402df | 1260 | |
slowness | 0:c03cffe402df | 1261 | |
slowness | 0:c03cffe402df | 1262 | // Wait for data to transmit |
slowness | 0:c03cffe402df | 1263 | wait (0.005); |
slowness | 0:c03cffe402df | 1264 | |
slowness | 0:c03cffe402df | 1265 | // make sure we have a valid return |
slowness | 0:c03cffe402df | 1266 | Status[4]=0x00; |
slowness | 0:c03cffe402df | 1267 | |
slowness | 0:c03cffe402df | 1268 | // we'll only get a reply if it was not broadcast |
slowness | 0:c03cffe402df | 1269 | if (_ID!=0xFE) { |
slowness | 0:c03cffe402df | 1270 | |
slowness | 0:c03cffe402df | 1271 | |
slowness | 0:c03cffe402df | 1272 | /* Partie de reception de la trame de retour |
slowness | 0:c03cffe402df | 1273 | */ |
slowness | 0:c03cffe402df | 1274 | while ((flag_out != 1) && (timeout < MAX_TIMEOUT)) |
slowness | 0:c03cffe402df | 1275 | { |
slowness | 0:c03cffe402df | 1276 | // Les differents etats de l'automate on été créés au debut de la fonction write ! |
slowness | 0:c03cffe402df | 1277 | switch (etat) |
slowness | 0:c03cffe402df | 1278 | { |
slowness | 0:c03cffe402df | 1279 | case Header1: if (_ax12.readable()) // reception du premier Header ( 0xFF ) |
slowness | 0:c03cffe402df | 1280 | { |
slowness | 0:c03cffe402df | 1281 | Status[plen] = _ax12.getc(); |
slowness | 0:c03cffe402df | 1282 | timeout = 0; |
slowness | 0:c03cffe402df | 1283 | if (Status[plen] == 0xFF ) |
slowness | 0:c03cffe402df | 1284 | { |
slowness | 0:c03cffe402df | 1285 | etat = Header2; |
slowness | 0:c03cffe402df | 1286 | #ifdef AX12_DEBUG_WRITE |
slowness | 0:c03cffe402df | 1287 | printf("data[%d] : %d\r\n", plen, (int)Status[plen]); |
slowness | 0:c03cffe402df | 1288 | #endif |
slowness | 0:c03cffe402df | 1289 | plen++; |
slowness | 0:c03cffe402df | 1290 | |
slowness | 0:c03cffe402df | 1291 | } |
slowness | 0:c03cffe402df | 1292 | else etat = Header1; |
slowness | 0:c03cffe402df | 1293 | } |
slowness | 0:c03cffe402df | 1294 | else timeout++; |
slowness | 0:c03cffe402df | 1295 | break; |
slowness | 0:c03cffe402df | 1296 | |
slowness | 0:c03cffe402df | 1297 | |
slowness | 0:c03cffe402df | 1298 | case Header2: if (_ax12.readable()) // reception du second Header ( 0xFF ) |
slowness | 0:c03cffe402df | 1299 | { |
slowness | 0:c03cffe402df | 1300 | Status[plen] = _ax12.getc(); |
slowness | 0:c03cffe402df | 1301 | timeout = 0; |
slowness | 0:c03cffe402df | 1302 | if (Status[plen] == 0xFF ) |
slowness | 0:c03cffe402df | 1303 | { |
slowness | 0:c03cffe402df | 1304 | etat = ident; |
slowness | 0:c03cffe402df | 1305 | #ifdef AX12_DEBUG_WRITE |
slowness | 0:c03cffe402df | 1306 | printf("data[%d] : %d\r\n", plen, (int)Status[plen]); |
slowness | 0:c03cffe402df | 1307 | #endif |
slowness | 0:c03cffe402df | 1308 | plen++; |
slowness | 0:c03cffe402df | 1309 | } |
slowness | 0:c03cffe402df | 1310 | else |
slowness | 0:c03cffe402df | 1311 | { |
slowness | 0:c03cffe402df | 1312 | etat = Header1; |
slowness | 0:c03cffe402df | 1313 | plen = 0; |
slowness | 0:c03cffe402df | 1314 | } |
slowness | 0:c03cffe402df | 1315 | } |
slowness | 0:c03cffe402df | 1316 | else timeout++; |
slowness | 0:c03cffe402df | 1317 | break; |
slowness | 0:c03cffe402df | 1318 | |
slowness | 0:c03cffe402df | 1319 | case ident: if (_ax12.readable()) // reception de l'octet correspondant à l'ID du servomoteur |
slowness | 0:c03cffe402df | 1320 | { |
slowness | 0:c03cffe402df | 1321 | Status[plen] = _ax12.getc(); |
slowness | 0:c03cffe402df | 1322 | timeout = 0; |
slowness | 0:c03cffe402df | 1323 | if (Status[plen] == ID ) |
slowness | 0:c03cffe402df | 1324 | { |
slowness | 0:c03cffe402df | 1325 | etat = length; |
slowness | 0:c03cffe402df | 1326 | #ifdef AX12_DEBUG_WRITE |
slowness | 0:c03cffe402df | 1327 | printf("data[%d] : %d\r\n", plen, (int)Status[plen]); |
slowness | 0:c03cffe402df | 1328 | #endif |
slowness | 0:c03cffe402df | 1329 | plen++; |
slowness | 0:c03cffe402df | 1330 | } |
slowness | 0:c03cffe402df | 1331 | else |
slowness | 0:c03cffe402df | 1332 | { |
slowness | 0:c03cffe402df | 1333 | etat = Header1; |
slowness | 0:c03cffe402df | 1334 | plen = 0; |
slowness | 0:c03cffe402df | 1335 | } |
slowness | 0:c03cffe402df | 1336 | } |
slowness | 0:c03cffe402df | 1337 | else timeout++; |
slowness | 0:c03cffe402df | 1338 | break; |
slowness | 0:c03cffe402df | 1339 | |
slowness | 0:c03cffe402df | 1340 | case length: if (_ax12.readable()) // reception de l'octet correspondant à la taille ( taille = 2 + nombre de paramètres ) |
slowness | 0:c03cffe402df | 1341 | { |
slowness | 0:c03cffe402df | 1342 | Status[plen] = _ax12.getc(); |
slowness | 0:c03cffe402df | 1343 | timeout = 0; |
slowness | 0:c03cffe402df | 1344 | 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 | 1345 | { |
slowness | 0:c03cffe402df | 1346 | etat = erreur; |
slowness | 0:c03cffe402df | 1347 | #ifdef AX12_DEBUG_WRITE |
slowness | 0:c03cffe402df | 1348 | printf("data[%d] : %d\r\n", plen, (int)Status[plen]); |
slowness | 0:c03cffe402df | 1349 | #endif |
slowness | 0:c03cffe402df | 1350 | plen++; |
slowness | 0:c03cffe402df | 1351 | } |
slowness | 0:c03cffe402df | 1352 | else |
slowness | 0:c03cffe402df | 1353 | { |
slowness | 0:c03cffe402df | 1354 | etat = Header1; |
slowness | 0:c03cffe402df | 1355 | plen = 0; |
slowness | 0:c03cffe402df | 1356 | } |
slowness | 0:c03cffe402df | 1357 | } |
slowness | 0:c03cffe402df | 1358 | else timeout++; |
slowness | 0:c03cffe402df | 1359 | break; |
slowness | 0:c03cffe402df | 1360 | |
slowness | 0:c03cffe402df | 1361 | case erreur: if (_ax12.readable()) //reception de l'octet correspondant au code d'erreurs eventuels ( 0 = pas d'erreur ) |
slowness | 0:c03cffe402df | 1362 | { |
slowness | 0:c03cffe402df | 1363 | Status[plen] = _ax12.getc(); |
slowness | 0:c03cffe402df | 1364 | timeout = 0; |
slowness | 0:c03cffe402df | 1365 | #ifdef AX12_DEBUG_WRITE |
slowness | 0:c03cffe402df | 1366 | printf("data[%d] : %d\r\n", plen, (int)Status[plen]); |
slowness | 0:c03cffe402df | 1367 | #endif |
slowness | 0:c03cffe402df | 1368 | plen++; |
slowness | 0:c03cffe402df | 1369 | etat = checksum; |
slowness | 0:c03cffe402df | 1370 | } |
slowness | 0:c03cffe402df | 1371 | else timeout++; |
slowness | 0:c03cffe402df | 1372 | |
slowness | 0:c03cffe402df | 1373 | 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 | 1374 | { |
slowness | 0:c03cffe402df | 1375 | Status[plen] = _ax12.getc(); |
slowness | 0:c03cffe402df | 1376 | timeout = 0; |
slowness | 0:c03cffe402df | 1377 | flag_out = 1; |
slowness | 0:c03cffe402df | 1378 | etat = Header1; |
slowness | 0:c03cffe402df | 1379 | #ifdef AX12_DEBUG_WRITE |
slowness | 0:c03cffe402df | 1380 | printf("data[%d] : %d\r\n\n", plen, (int)Status[plen]); |
slowness | 0:c03cffe402df | 1381 | #endif |
slowness | 0:c03cffe402df | 1382 | } |
slowness | 0:c03cffe402df | 1383 | else timeout++; |
slowness | 0:c03cffe402df | 1384 | break; |
slowness | 0:c03cffe402df | 1385 | } |
slowness | 0:c03cffe402df | 1386 | } |
slowness | 0:c03cffe402df | 1387 | |
slowness | 0:c03cffe402df | 1388 | |
slowness | 0:c03cffe402df | 1389 | if ( Status[4] != 0 ) |
slowness | 0:c03cffe402df | 1390 | { |
slowness | 0:c03cffe402df | 1391 | #ifdef AX12_DEBUG |
slowness | 0:c03cffe402df | 1392 | printf ("erreur ! \r\n"); |
slowness | 0:c03cffe402df | 1393 | #endif |
slowness | 0:c03cffe402df | 1394 | for (int i = 0; i<5; i++) |
slowness | 0:c03cffe402df | 1395 | { |
slowness | 0:c03cffe402df | 1396 | #ifdef AX12_DEBUG |
slowness | 0:c03cffe402df | 1397 | printf("data[%d] : %d\r\n", plen, (int)Status[plen]); |
slowness | 0:c03cffe402df | 1398 | #endif |
slowness | 0:c03cffe402df | 1399 | } |
slowness | 0:c03cffe402df | 1400 | } |
slowness | 0:c03cffe402df | 1401 | |
slowness | 0:c03cffe402df | 1402 | 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 | 1403 | { |
slowness | 0:c03cffe402df | 1404 | #ifdef AX12_DEBUG |
slowness | 0:c03cffe402df | 1405 | printf ("timeout erreur\n\r"); |
slowness | 0:c03cffe402df | 1406 | #endif |
slowness | 0:c03cffe402df | 1407 | return(-1); |
slowness | 0:c03cffe402df | 1408 | } |
slowness | 0:c03cffe402df | 1409 | |
slowness | 0:c03cffe402df | 1410 | // Build the TxPacket first in RAM, then we'll send in one go |
slowness | 0:c03cffe402df | 1411 | } |
slowness | 0:c03cffe402df | 1412 | |
slowness | 0:c03cffe402df | 1413 | return(Status[4]); // retourne le code d'erreur ( octect 5 de la trame de retour ) |
slowness | 0:c03cffe402df | 1414 | } |