CRAC Team / CRAC-Strat_2017_homologation_gros_rob_testAck

homologation gros robot et test avec les ack de la carte a tout faire

Fork of CRAC-Strat_2017_HOMOLOGATION_PETIT_ROBOT by CRAC Team

AX12/AX12.cpp@15:c2fc239e85df, 2017-05-11 (annotated)

Committer:
ClementBreteau
Date:
Thu May 11 12:55:52 2017 +0000
Revision:
15:c2fc239e85df
Child:
16:7321fb3bb396
crac strat le 11/05/17

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