CRAC Team / Utilisatio_MX12_V2

Programme d'utilisation des AX12 avec rajout de l'MX12

Fork of test_carteAToutFaire_PR by CRAC Team

AX12/AX12.cpp@1:f3f702086a30, 2017-05-11 (annotated)

Committer:
ClementBreteau
Date:
Thu May 11 11:49:50 2017 +0000
Revision:
1:f3f702086a30
test de la carte ? tout faire du petit robot

Who changed what in which revision?

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