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