CRAC Team / Utilisation_MX12

Programme d'utilisation servomotors MX12 V1

AX12/AX12.cpp@0:80df663dd15e, 2017-05-19 (annotated)

Committer:
R66Y
Date:
Fri May 19 14:32:14 2017 +0000
Revision:
0:80df663dd15e
programme pour utiliser les servomoteurs MX12.

Who changed what in which revision?

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