clement lignie / configure_ax12_test_bras_module

config AX12

Fork of configure_ax12_test_bras_module by CRAC Team

AX12.cpp@2:91b6646ea994, 2017-02-01 (annotated)

Committer:
clementlignie
Date:
Wed Feb 01 13:42:30 2017 +0000
Revision:
2:91b6646ea994
Parent:
1:fce3323d874f 
config AX12

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