clement lignie / configure_ax12_test_bras_module

config AX12

Fork of configure_ax12_test_bras_module by CRAC Team

AX12.cpp@0:c03cffe402df, 2016-02-03 (annotated)

Committer:
slowness
Date:
Wed Feb 03 14:01:53 2016 +0000
Revision:
0:c03cffe402df
Child:
1:fce3323d874f
Pour configurer les AX12 avec la carte NXP1768 sur les Pin 9 et 10

Who changed what in which revision?

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