CRAC Team / CRAC-Strat_2017-19-05-2017

strat des robots

Fork of CRAC-Strat_2017 by CRAC Team

AX12/AX12.cpp@17:d1594579eec6, 2017-05-19 (annotated)

Committer:
ClementBreteau
Date:
Fri May 19 17:14:07 2017 +0000
Revision:
17:d1594579eec6
Parent:
16:7321fb3bb396 
strat du robot, 19-05-2017, 19h

Who changed what in which revision?

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