CRAC Team / Mbed 2 deprecated carteAToutFaire

code de la carte à tout faire / carte strat

Dependencies:   mbed

AX12/AX12.cpp@0:76bc3ed27822, 2017-05-10 (annotated)

Committer:
ClementBreteau
Date:
Wed May 10 09:10:26 2017 +0000
Revision:
0:76bc3ed27822
pour gatien le 10 5 2017

Who changed what in which revision?

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