carte esclave petit robot

Dependencies:   mbed Herkulex_Library_2019 actions_Gr ident_crac actions_Pr

Committer:
Artiom
Date:
Wed May 22 08:25:51 2019 +0000
Revision:
20:42f8ec726ac8
Parent:
19:511ff570c2f5
Child:
21:edf6d03fdb20

        

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Artiom 0:bc74da1c502f 1 #include "mbed.h"
Artiom 0:bc74da1c502f 2 #include "ident_crac.h"
Artiom 0:bc74da1c502f 3 #include "Capteur.h"
Artiom 0:bc74da1c502f 4 #include "Actionneurs.h"
Artiom 0:bc74da1c502f 5 #include "fonctions_herkulex.h"
Artiom 1:568955af8c2b 6 #include "main.h"
Artiom 9:9833e788942b 7 #include "dt.h"
Artiom 0:bc74da1c502f 8
Artiom 0:bc74da1c502f 9 #define SIZE_FIFO 50
Artiom 0:bc74da1c502f 10
Artiom 0:bc74da1c502f 11 CAN can(PB_8,PB_9,1000000); // Rx&Tx pour le CAN
Artiom 0:bc74da1c502f 12
Artiom 0:bc74da1c502f 13
Artiom 0:bc74da1c502f 14 CANMessage msgRxBuffer[SIZE_FIFO];
Artiom 0:bc74da1c502f 15 unsigned char FIFO_ecriture=0; //Position du fifo pour la reception CAN
Artiom 0:bc74da1c502f 16 signed char FIFO_lecture=0;//Position du fifo de lecture des messages CAN
Artiom 0:bc74da1c502f 17 unsigned char EtatGameEnd=0;
Artiom 0:bc74da1c502f 18
Artiom 6:45f9cf44718a 19 char fpresentoir_avant=0, fpresentoir_arriere=0;
Artiom 14:b1abd2f6df75 20 char fgoldenium_avant=0, fgoldenium_arriere=0;
Artiom 6:45f9cf44718a 21 char fsol_avant=0,fsol_arriere=0;
Artiom 7:553f3f1c2c53 22 char fsol_avant_relache=0,fsol_arriere_relache=0;
Artiom 12:191486ba5118 23 char fbalance_avant=0,fbalance_arriere=0;
Artiom 12:191486ba5118 24 char favant_relache=0,farriere_relache=0;
Artiom 14:b1abd2f6df75 25 char faccelerateur_avant=0,faccelerateur_arriere=0;;
Artiom 12:191486ba5118 26
Artiom 12:191486ba5118 27
Artiom 13:9e19048eb065 28
Artiom 4:4a79942713fa 29 char status_pompe=0;
Artiom 4:4a79942713fa 30
Artiom 0:bc74da1c502f 31 void canRx_ISR (void);
Artiom 0:bc74da1c502f 32 void SendAck(unsigned short id, unsigned short from);
Artiom 0:bc74da1c502f 33 void SendRawId (unsigned short id);
Artiom 0:bc74da1c502f 34 void GoStraight (signed short distance,unsigned char recalage, unsigned short newValue, unsigned char isEnchainement);
Artiom 0:bc74da1c502f 35 void canProcessRx(void);
Artiom 6:45f9cf44718a 36
Artiom 4:4a79942713fa 37 void automate_ventouse_presentoir_avant(void);
Artiom 5:6e198cdd99ad 38 void automate_ventouse_presentoir_arriere (void);
Artiom 0:bc74da1c502f 39
Artiom 6:45f9cf44718a 40 void automate_ventouse_goldenium_avant (void);
Artiom 6:45f9cf44718a 41 void automate_ventouse_goldenium_arriere (void);
Artiom 6:45f9cf44718a 42
Artiom 6:45f9cf44718a 43 void automate_ventouse_sol_avant (void);
Artiom 6:45f9cf44718a 44 void automate_ventouse_sol_arriere(void);
Artiom 6:45f9cf44718a 45
Artiom 7:553f3f1c2c53 46 void automate_ventouse_sol_avant_relache(void);
Artiom 7:553f3f1c2c53 47 void automate_ventouse_sol_arriere_relache(void);
Artiom 11:6c11c081c855 48
Artiom 11:6c11c081c855 49 void automate_ventouse_balance_avant (void);
Artiom 12:191486ba5118 50 void automate_ventouse_balance_arriere(void);
Artiom 12:191486ba5118 51
Artiom 13:9e19048eb065 52 void automate_ventouse_relache_avant(void);
Artiom 13:9e19048eb065 53 void automate_ventouse_relache_arriere (void);
Artiom 12:191486ba5118 54
Artiom 14:b1abd2f6df75 55 void automate_ventouse_accelerateur_avant(void);
Artiom 14:b1abd2f6df75 56 void automate_ventouse_accelerateur_arriere(void);
Artiom 14:b1abd2f6df75 57
Artiom 14:b1abd2f6df75 58
Artiom 0:bc74da1c502f 59 /*********************************************************************************************/
Artiom 0:bc74da1c502f 60 /* FUNCTION NAME: canRx_ISR */
Artiom 0:bc74da1c502f 61 /* DESCRIPTION : lit les messages sur le can et les stocke dans la FIFO */
Artiom 0:bc74da1c502f 62 /*********************************************************************************************/
Artiom 0:bc74da1c502f 63 void canRx_ISR (void)
Artiom 0:bc74da1c502f 64 {
Artiom 0:bc74da1c502f 65 if (can.read(msgRxBuffer[FIFO_ecriture])) {
Artiom 0:bc74da1c502f 66 if (msgRxBuffer[FIFO_ecriture].id==GLOBAL_GAME_END) {
Artiom 0:bc74da1c502f 67
Artiom 0:bc74da1c502f 68 }
Artiom 0:bc74da1c502f 69 FIFO_ecriture=(FIFO_ecriture+1)%SIZE_FIFO;
Artiom 0:bc74da1c502f 70
Artiom 0:bc74da1c502f 71 }
Artiom 0:bc74da1c502f 72
Artiom 0:bc74da1c502f 73 }
Artiom 0:bc74da1c502f 74 /*********************************************************************************************/
Artiom 0:bc74da1c502f 75 /* FUNCTION NAME: SendAck */
Artiom 0:bc74da1c502f 76 /* DESCRIPTION : */
Artiom 0:bc74da1c502f 77 /*********************************************************************************************/
Artiom 0:bc74da1c502f 78 void SendAck(unsigned short from, unsigned short id) //FROM et ID sont inversés pour convenir à la lecture d'ACK de la carte principale
Artiom 0:bc74da1c502f 79 {
Artiom 0:bc74da1c502f 80 CANMessage msgTx=CANMessage();
Artiom 0:bc74da1c502f 81 msgTx.id=from; //waitingAckFrom
Artiom 0:bc74da1c502f 82 msgTx.len=2;
Artiom 0:bc74da1c502f 83 msgTx.format=CANStandard;
Artiom 0:bc74da1c502f 84 msgTx.type=CANData;
Artiom 0:bc74da1c502f 85 // from sur 2 octets
Artiom 0:bc74da1c502f 86 msgTx.data[0]=(unsigned char)id; //waitingAckID
Artiom 0:bc74da1c502f 87 msgTx.data[1]=(unsigned char)(id>>8);
Artiom 0:bc74da1c502f 88 can.write(msgTx);
Artiom 0:bc74da1c502f 89 }
Artiom 0:bc74da1c502f 90 /*********************************************************************************************/
Artiom 0:bc74da1c502f 91 /* FUNCTION NAME: SendRawId */
Artiom 0:bc74da1c502f 92 /* DESCRIPTION : */
Artiom 0:bc74da1c502f 93 /*********************************************************************************************/
Artiom 0:bc74da1c502f 94 void SendRawId (unsigned short id)
Artiom 0:bc74da1c502f 95 {
Artiom 0:bc74da1c502f 96 CANMessage msgTx=CANMessage();
Artiom 0:bc74da1c502f 97 msgTx.id=id;
Artiom 0:bc74da1c502f 98 msgTx.len=0;
Artiom 0:bc74da1c502f 99 can.write(msgTx); //carte esclave f446re
Artiom 0:bc74da1c502f 100 wait_us(200);
Artiom 0:bc74da1c502f 101 }
Artiom 0:bc74da1c502f 102
Artiom 0:bc74da1c502f 103 /*********************************************************************************************/
Artiom 0:bc74da1c502f 104 /* FUNCTION NAME: GoStraight */
Artiom 0:bc74da1c502f 105 /* DESCRIPTION : Transmission CAN correspondant à une ligne droite, avec ou sans recalage */
Artiom 0:bc74da1c502f 106 /* recalage : 0 => pas de recalage */
Artiom 0:bc74da1c502f 107 /* 1 => recalage en X */
Artiom 0:bc74da1c502f 108 /* 2 => Recalage en Y */
Artiom 0:bc74da1c502f 109 /* newValue : Uniquement en cas de recalage, indique la nouvelle valeur de l'odo */
Artiom 0:bc74da1c502f 110 /* isEnchainement : Indique si il faut executer l'instruction en enchainement */
Artiom 0:bc74da1c502f 111 /* 0 => non */
Artiom 0:bc74da1c502f 112 /* 1 => oui */
Artiom 0:bc74da1c502f 113 /* 2 => dernière instruction de l'enchainement */
Artiom 0:bc74da1c502f 114 /*********************************************************************************************/
Artiom 0:bc74da1c502f 115 void GoStraight (signed short distance,unsigned char recalage, unsigned short newValue, unsigned char isEnchainement)
Artiom 0:bc74da1c502f 116 {
Artiom 0:bc74da1c502f 117 CANMessage msgTx=CANMessage();
Artiom 0:bc74da1c502f 118 msgTx.id=ASSERVISSEMENT_RECALAGE;
Artiom 0:bc74da1c502f 119 msgTx.len=6;
Artiom 0:bc74da1c502f 120 msgTx.format=CANStandard;
Artiom 0:bc74da1c502f 121 msgTx.type=CANData;
Artiom 0:bc74da1c502f 122 // x sur 2 octets
Artiom 0:bc74da1c502f 123 msgTx.data[0]=(unsigned char)distance;
Artiom 0:bc74da1c502f 124 msgTx.data[1]=(unsigned char)(distance>>8);
Artiom 0:bc74da1c502f 125 //Recalage sur 1 octet
Artiom 0:bc74da1c502f 126 msgTx.data[2]=recalage;
Artiom 0:bc74da1c502f 127 //Valeur du recalage sur 2 octets
Artiom 0:bc74da1c502f 128 msgTx.data[3]=(unsigned char)newValue;
Artiom 0:bc74da1c502f 129 msgTx.data[4]=(unsigned char)(newValue>>8);
Artiom 0:bc74da1c502f 130 //Enchainement sur 1 octet
Artiom 0:bc74da1c502f 131 msgTx.data[5]=isEnchainement;
Artiom 0:bc74da1c502f 132
Artiom 0:bc74da1c502f 133 can.write(msgTx);
Artiom 0:bc74da1c502f 134 //wait_ms(500);
Artiom 0:bc74da1c502f 135 }
Artiom 12:191486ba5118 136 /****************************************************************************************/
Artiom 12:191486ba5118 137 /* FUNCTION NAME: Rotate */
Artiom 12:191486ba5118 138 /* DESCRIPTION : Transmission CAN correspondant à une rotation */
Artiom 12:191486ba5118 139 /****************************************************************************************/
Artiom 12:191486ba5118 140 void Rotate (signed short angle)
Artiom 12:191486ba5118 141 {
Artiom 12:191486ba5118 142 CANMessage msgTx=CANMessage();
Artiom 12:191486ba5118 143 msgTx.id=ASSERVISSEMENT_ROTATION; // Tx rotation autour du centre du robot
Artiom 12:191486ba5118 144 msgTx.len=2;
Artiom 12:191486ba5118 145 msgTx.format=CANStandard;
Artiom 12:191486ba5118 146 msgTx.type=CANData;
Artiom 12:191486ba5118 147 // Angle signé sur 2 octets
Artiom 12:191486ba5118 148 msgTx.data[0]=(unsigned char)angle;
Artiom 12:191486ba5118 149 msgTx.data[1]=(unsigned char)(angle>>8);
Artiom 12:191486ba5118 150
Artiom 12:191486ba5118 151 can.write(msgTx);
Artiom 12:191486ba5118 152 }
Artiom 0:bc74da1c502f 153
Artiom 0:bc74da1c502f 154 int main()
Artiom 0:bc74da1c502f 155 {
Artiom 0:bc74da1c502f 156 can.attach(&canRx_ISR); // création de l'interrupt attachée à la réception sur le CAN
Artiom 0:bc74da1c502f 157 servo_interrupt_en(); //permettre les interuptions
Artiom 11:6c11c081c855 158
Artiom 0:bc74da1c502f 159 wait(1);//attente servo boot
Artiom 2:9e63099cca99 160 gabarit_petit_robot();
Artiom 0:bc74da1c502f 161 while(1) {
Artiom 0:bc74da1c502f 162 canProcessRx();
Artiom 10:48c0d1b8aed9 163 f_mesure();//dt35
Artiom 11:6c11c081c855 164
Artiom 4:4a79942713fa 165 automate_ventouse_presentoir_avant();
Artiom 5:6e198cdd99ad 166 automate_ventouse_presentoir_arriere();
Artiom 7:553f3f1c2c53 167
Artiom 6:45f9cf44718a 168 automate_ventouse_goldenium_avant();
Artiom 6:45f9cf44718a 169 automate_ventouse_goldenium_arriere();
Artiom 7:553f3f1c2c53 170
Artiom 6:45f9cf44718a 171 automate_ventouse_sol_avant();
Artiom 7:553f3f1c2c53 172 automate_ventouse_sol_arriere();
Artiom 7:553f3f1c2c53 173
Artiom 11:6c11c081c855 174 automate_ventouse_balance_avant();
Artiom 12:191486ba5118 175 automate_ventouse_balance_arriere();
Artiom 12:191486ba5118 176
Artiom 13:9e19048eb065 177 automate_ventouse_relache_avant();
Artiom 12:191486ba5118 178 automate_ventouse_relache_arriere();
Artiom 14:b1abd2f6df75 179
Artiom 13:9e19048eb065 180 automate_ventouse_sol_avant_relache();
Artiom 14:b1abd2f6df75 181 automate_ventouse_sol_arriere_relache();
Artiom 14:b1abd2f6df75 182
Artiom 14:b1abd2f6df75 183 automate_ventouse_accelerateur_avant();
Artiom 14:b1abd2f6df75 184 automate_ventouse_accelerateur_arriere();
Artiom 13:9e19048eb065 185
Artiom 0:bc74da1c502f 186 if(EtatGameEnd==1) {
Artiom 0:bc74da1c502f 187 while(1);
Artiom 0:bc74da1c502f 188 }
Artiom 0:bc74da1c502f 189
Artiom 0:bc74da1c502f 190 }
Artiom 0:bc74da1c502f 191 }
Artiom 0:bc74da1c502f 192
Artiom 0:bc74da1c502f 193 /****************************************************************************************/
Artiom 0:bc74da1c502f 194 /* FUNCTION NAME: canProcessRx */
Artiom 0:bc74da1c502f 195 /* DESCRIPTION : Fonction de traitement des messages CAN */
Artiom 0:bc74da1c502f 196 /****************************************************************************************/
Artiom 0:bc74da1c502f 197 void canProcessRx(void)
Artiom 0:bc74da1c502f 198 {
Artiom 0:bc74da1c502f 199 static signed char FIFO_occupation=0,FIFO_max_occupation=0;
Artiom 0:bc74da1c502f 200 CANMessage msgTx=CANMessage();
Artiom 0:bc74da1c502f 201 FIFO_occupation=FIFO_ecriture-FIFO_lecture;
Artiom 0:bc74da1c502f 202 if(FIFO_occupation<0)
Artiom 0:bc74da1c502f 203 FIFO_occupation=FIFO_occupation+SIZE_FIFO;
Artiom 0:bc74da1c502f 204 if(FIFO_max_occupation<FIFO_occupation)
Artiom 0:bc74da1c502f 205 FIFO_max_occupation=FIFO_occupation;
Artiom 0:bc74da1c502f 206 if(FIFO_occupation!=0) {
Artiom 0:bc74da1c502f 207 int identifiant=msgRxBuffer[FIFO_lecture].id;
Artiom 0:bc74da1c502f 208
Artiom 0:bc74da1c502f 209 switch(identifiant) {
Artiom 0:bc74da1c502f 210
Artiom 0:bc74da1c502f 211 case GLOBAL_GAME_END:
Artiom 0:bc74da1c502f 212 EtatGameEnd = 1;
Artiom 0:bc74da1c502f 213 break;
Artiom 0:bc74da1c502f 214
Artiom 0:bc74da1c502f 215 case DATA_TELEMETRE: //Lit le telemetre N°X suivant la data dans le CAN
Artiom 0:bc74da1c502f 216 char numero_telemetre=msgRxBuffer[FIFO_lecture].data[0];
Artiom 0:bc74da1c502f 217 short distance=lecture_telemetre(numero_telemetre);
Artiom 0:bc74da1c502f 218
Artiom 0:bc74da1c502f 219
Artiom 0:bc74da1c502f 220 msgTx.id=RECEPTION_DATA; // tx Valeur Telemetre1
Artiom 0:bc74da1c502f 221 msgTx.len=2;
Artiom 0:bc74da1c502f 222 msgTx.format=CANStandard;
Artiom 0:bc74da1c502f 223 msgTx.type=CANData;
Artiom 0:bc74da1c502f 224 // Rayon sur 2 octets
Artiom 0:bc74da1c502f 225 msgTx.data[0]=(unsigned char)distance;
Artiom 0:bc74da1c502f 226 msgTx.data[1]=(unsigned char)(distance>>8);
Artiom 0:bc74da1c502f 227
Artiom 0:bc74da1c502f 228 can.write(msgTx);
Artiom 0:bc74da1c502f 229 SendAck(ACKNOWLEDGE_TELEMETRE,RECEPTION_DATA);
Artiom 0:bc74da1c502f 230 break;
Artiom 0:bc74da1c502f 231
Artiom 0:bc74da1c502f 232 case DATA_RECALAGE:
Artiom 0:bc74da1c502f 233 short distance1=lecture_telemetre(1);
Artiom 0:bc74da1c502f 234 short distance2=lecture_telemetre(2);
Artiom 0:bc74da1c502f 235 short distance3=lecture_telemetre(3);
Artiom 0:bc74da1c502f 236 short distance4=lecture_telemetre(4);
Artiom 0:bc74da1c502f 237
Artiom 0:bc74da1c502f 238 msgTx.id=RECEPTION_RECALAGE; // tx Valeur Telemetre1
Artiom 0:bc74da1c502f 239 msgTx.len=8;
Artiom 0:bc74da1c502f 240 msgTx.format=CANStandard;
Artiom 0:bc74da1c502f 241 msgTx.type=CANData;
Artiom 0:bc74da1c502f 242 // Rayon sur 2 octets
Artiom 0:bc74da1c502f 243 msgTx.data[0]=(unsigned char)distance1;
Artiom 0:bc74da1c502f 244 msgTx.data[1]=(unsigned char)(distance1>>8);
Artiom 0:bc74da1c502f 245 msgTx.data[2]=(unsigned char)distance2;
Artiom 0:bc74da1c502f 246 msgTx.data[3]=(unsigned char)(distance2>>8);
Artiom 0:bc74da1c502f 247 msgTx.data[4]=(unsigned char)distance3;
Artiom 0:bc74da1c502f 248 msgTx.data[5]=(unsigned char)(distance3>>8);
Artiom 0:bc74da1c502f 249 msgTx.data[6]=(unsigned char)distance4;
Artiom 0:bc74da1c502f 250 msgTx.data[7]=(unsigned char)(distance4>>8);
Artiom 0:bc74da1c502f 251 can.write(msgTx);
Artiom 0:bc74da1c502f 252 SendAck(ACKNOWLEDGE_TELEMETRE,RECEPTION_RECALAGE);
Artiom 0:bc74da1c502f 253 break;
Artiom 0:bc74da1c502f 254
Artiom 0:bc74da1c502f 255 #ifdef ROBOT_SMALL
Artiom 4:4a79942713fa 256 //-------------------------------------------------------------------------Actions petit robot----------------------------------------------
Artiom 2:9e63099cca99 257 case GABARIT_PETIT_ROBOT:
Artiom 2:9e63099cca99 258 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom 2:9e63099cca99 259 gabarit_petit_robot();
Artiom 2:9e63099cca99 260 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom 0:bc74da1c502f 261 break;
Artiom 0:bc74da1c502f 262
Artiom 0:bc74da1c502f 263 case PRESENTOIR_AVANT:
Artiom 4:4a79942713fa 264 fpresentoir_avant=1;
Artiom 0:bc74da1c502f 265 break;
Artiom 0:bc74da1c502f 266
Artiom 0:bc74da1c502f 267 case PRESENTOIR_ARRIERE:
Artiom 5:6e198cdd99ad 268 fpresentoir_arriere=1;
Artiom 0:bc74da1c502f 269 break;
Artiom 0:bc74da1c502f 270
Artiom 0:bc74da1c502f 271 case BALANCE_AVANT:
Artiom 11:6c11c081c855 272 fbalance_avant=1;
Artiom 0:bc74da1c502f 273 break;
Artiom 0:bc74da1c502f 274
Artiom 0:bc74da1c502f 275 case BALANCE_ARRIERE:
Artiom 12:191486ba5118 276 fbalance_arriere=1;
Artiom 2:9e63099cca99 277 break;
Artiom 2:9e63099cca99 278
Artiom 2:9e63099cca99 279 case ACCELERATEUR_AVANT:
Artiom 14:b1abd2f6df75 280 faccelerateur_avant=1;
Artiom 2:9e63099cca99 281 break;
Artiom 2:9e63099cca99 282
Artiom 2:9e63099cca99 283 case ACCELERATEUR_ARRIERE:
Artiom 14:b1abd2f6df75 284 faccelerateur_arriere=1;
Artiom 2:9e63099cca99 285 break;
Artiom 6:45f9cf44718a 286
Artiom 2:9e63099cca99 287 case GOLDENIUM_AVANT:
Artiom 6:45f9cf44718a 288 fgoldenium_avant=1;
Artiom 2:9e63099cca99 289 break;
Artiom 2:9e63099cca99 290
Artiom 2:9e63099cca99 291 case GOLDENIUM_ARRIERE:
Artiom 6:45f9cf44718a 292 fgoldenium_arriere=1;
Artiom 2:9e63099cca99 293 break;
Artiom 6:45f9cf44718a 294
Artiom 2:9e63099cca99 295 case SOL_AVANT:
Artiom 6:45f9cf44718a 296 fsol_avant=1;
Artiom 2:9e63099cca99 297 break;
Artiom 2:9e63099cca99 298
Artiom 2:9e63099cca99 299 case SOL_ARRIERE:
Artiom 6:45f9cf44718a 300 fsol_arriere=1;
Artiom 2:9e63099cca99 301 break;
Artiom 2:9e63099cca99 302
Artiom 2:9e63099cca99 303 case SOL_AVANT_RELACHE:
Artiom 7:553f3f1c2c53 304 fsol_avant_relache=1;
Artiom 2:9e63099cca99 305 break;
Artiom 2:9e63099cca99 306
Artiom 2:9e63099cca99 307 case SOL_ARRIERE_RELACHE:
Artiom 7:553f3f1c2c53 308 fsol_arriere_relache=1;
Artiom 0:bc74da1c502f 309 break;
Artiom 12:191486ba5118 310 case AVANT_RELACHE:
Artiom 12:191486ba5118 311 favant_relache=1;
Artiom 12:191486ba5118 312 break;
Artiom 12:191486ba5118 313
Artiom 12:191486ba5118 314 case ARRIERE_RELACHE:
Artiom 12:191486ba5118 315 farriere_relache=1;
Artiom 12:191486ba5118 316 break;
Artiom 14:b1abd2f6df75 317 case RECROQUEVILLER:
Artiom 14:b1abd2f6df75 318 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom 14:b1abd2f6df75 319 recroqueviller_avant();
Artiom 14:b1abd2f6df75 320 recroqueviller_arriere();
Artiom 14:b1abd2f6df75 321 verification();
Artiom 14:b1abd2f6df75 322 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom 14:b1abd2f6df75 323 break;
Artiom 13:9e19048eb065 324
Artiom 20:42f8ec726ac8 325 case VENTOUSE_AV_CENTRE_BALANCE:
Artiom 20:42f8ec726ac8 326 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom 20:42f8ec726ac8 327 goldenium_avant();
Artiom 20:42f8ec726ac8 328 positionControl(AV_poigne_D, 270,1,BLED_ON,1);//actionneur
Artiom 20:42f8ec726ac8 329 verification();
Artiom 20:42f8ec726ac8 330 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom 20:42f8ec726ac8 331 break;
Artiom 20:42f8ec726ac8 332
Artiom 20:42f8ec726ac8 333 case VENTOUSE_AR_CENTRE_BALANCE:
Artiom 20:42f8ec726ac8 334 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom 20:42f8ec726ac8 335 goldenium_arriere();
Artiom 20:42f8ec726ac8 336 positionControl(AR_poigne_D, 200,1,BLED_ON,3);//actionneur
Artiom 20:42f8ec726ac8 337 verification();
Artiom 20:42f8ec726ac8 338 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom 20:42f8ec726ac8 339 break;
Artiom 13:9e19048eb065 340
Artiom 4:4a79942713fa 341 //--------------------------------------------------------------------------ACK carte pompe----------------------------------------------
Artiom 4:4a79942713fa 342
Artiom 4:4a79942713fa 343
Artiom 4:4a79942713fa 344 case HACHEUR_STATUT_VENTOUSES:
Artiom 4:4a79942713fa 345 status_pompe = msgRxBuffer[FIFO_lecture].data[1];
Artiom 5:6e198cdd99ad 346 //can.write(CANMessage(0x529, &status_pompe,1));
Artiom 4:4a79942713fa 347 break;
Artiom 4:4a79942713fa 348
Artiom 4:4a79942713fa 349 case HACHEUR_GET_ATOM_ACK:
Artiom 4:4a79942713fa 350 status_pompe |= (0x01 << msgRxBuffer[FIFO_lecture].data[0]);
Artiom 5:6e198cdd99ad 351 //can.write(CANMessage(0x529, &status_pompe,1));
Artiom 4:4a79942713fa 352 break;
Artiom 4:4a79942713fa 353
Artiom 4:4a79942713fa 354 case HACHEUR_RELEASE_ATOM_ACK :
Artiom 4:4a79942713fa 355 status_pompe &= ~(0x01 << msgRxBuffer[FIFO_lecture].data[0]);
Artiom 4:4a79942713fa 356 break;
Artiom 13:9e19048eb065 357
Artiom 13:9e19048eb065 358
Artiom 4:4a79942713fa 359 //-------------------------------------------------------------------------------------------------------------------------------------------
Artiom 4:4a79942713fa 360
Artiom 0:bc74da1c502f 361
Artiom 0:bc74da1c502f 362 #endif
Artiom 0:bc74da1c502f 363 #ifdef ROBOT_BIG
kyxstark 18:0ea0d887966f 364 case COUROIES:
kyxstark 18:0ea0d887966f 365 rotation_couroies = msgRxBuffer[FIFO_lecture].data[0];
Artiom 0:bc74da1c502f 366 break;
Artiom 0:bc74da1c502f 367
Artiom 0:bc74da1c502f 368 case 5:
Artiom 0:bc74da1c502f 369
Artiom 0:bc74da1c502f 370 break;
Artiom 0:bc74da1c502f 371
Artiom 0:bc74da1c502f 372 #endif
Artiom 0:bc74da1c502f 373 }
Artiom 0:bc74da1c502f 374 FIFO_lecture=(FIFO_lecture+1)%SIZE_FIFO;
Artiom 0:bc74da1c502f 375
Artiom 0:bc74da1c502f 376 }
Artiom 4:4a79942713fa 377
Artiom 4:4a79942713fa 378
Artiom 4:4a79942713fa 379 }
Artiom 4:4a79942713fa 380
Artiom 4:4a79942713fa 381 void automate_ventouse_presentoir_avant (void)
Artiom 4:4a79942713fa 382 {
Artiom 4:4a79942713fa 383
Artiom 4:4a79942713fa 384 typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat;
Artiom 4:4a79942713fa 385 static type_etat etat = init;
Artiom 4:4a79942713fa 386
Artiom 4:4a79942713fa 387 switch(etat) {
Artiom 4:4a79942713fa 388 case init: //attente d'initialisation
Artiom 4:4a79942713fa 389 if(fpresentoir_avant)
Artiom 4:4a79942713fa 390 etat=envoi_instruction;
Artiom 4:4a79942713fa 391 break;
Artiom 4:4a79942713fa 392
Artiom 4:4a79942713fa 393 case envoi_instruction://envoi instruction
Artiom 4:4a79942713fa 394 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom 4:4a79942713fa 395 presentoir_avant();
Artiom 20:42f8ec726ac8 396 can.write(CANMessage(HACHEUR_GET_PRESENTOIR_AV));
Artiom 5:6e198cdd99ad 397 SendRawId(HACHEUR_STATUT_VENTOUSES);
Artiom 4:4a79942713fa 398 etat = attente_ack_ventouse;
Artiom 4:4a79942713fa 399 break;
Artiom 4:4a79942713fa 400
Artiom 4:4a79942713fa 401 case attente_ack_ventouse:
Artiom 5:6e198cdd99ad 402 if((status_pompe&MASK_PRESENTOIR_AV)==MASK_PRESENTOIR_AV) {
Artiom 4:4a79942713fa 403 fpresentoir_avant=0;
Artiom 5:6e198cdd99ad 404 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom 4:4a79942713fa 405 etat = init;
Artiom 5:6e198cdd99ad 406 }
Artiom 5:6e198cdd99ad 407 break;
Artiom 5:6e198cdd99ad 408
Artiom 5:6e198cdd99ad 409 }
Artiom 5:6e198cdd99ad 410 }
Artiom 5:6e198cdd99ad 411 void automate_ventouse_presentoir_arriere (void)
Artiom 5:6e198cdd99ad 412 {
Artiom 5:6e198cdd99ad 413
Artiom 5:6e198cdd99ad 414 typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat;
Artiom 5:6e198cdd99ad 415 static type_etat etat = init;
Artiom 5:6e198cdd99ad 416
Artiom 5:6e198cdd99ad 417 switch(etat) {
Artiom 5:6e198cdd99ad 418 case init: //attente d'initialisation
Artiom 5:6e198cdd99ad 419 if(fpresentoir_arriere)
Artiom 5:6e198cdd99ad 420 etat=envoi_instruction;
Artiom 5:6e198cdd99ad 421 break;
Artiom 5:6e198cdd99ad 422
Artiom 5:6e198cdd99ad 423 case envoi_instruction://envoi instruction
Artiom 5:6e198cdd99ad 424 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom 5:6e198cdd99ad 425 presentoir_arriere();
Artiom 20:42f8ec726ac8 426 can.write(CANMessage(HACHEUR_GET_PRESENTOIR_AR));
Artiom 5:6e198cdd99ad 427 SendRawId(HACHEUR_STATUT_VENTOUSES);
Artiom 5:6e198cdd99ad 428 etat = attente_ack_ventouse;
Artiom 5:6e198cdd99ad 429 break;
Artiom 5:6e198cdd99ad 430
Artiom 5:6e198cdd99ad 431 case attente_ack_ventouse:
Artiom 5:6e198cdd99ad 432 if((status_pompe&MASK_PRESENTOIR_AR)==MASK_PRESENTOIR_AR) {
Artiom 5:6e198cdd99ad 433 fpresentoir_arriere=0;
Artiom 5:6e198cdd99ad 434 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom 5:6e198cdd99ad 435 etat = init;
Artiom 4:4a79942713fa 436 }
Artiom 4:4a79942713fa 437 break;
Artiom 4:4a79942713fa 438
Artiom 4:4a79942713fa 439 }
Artiom 6:45f9cf44718a 440 }
Artiom 6:45f9cf44718a 441 void automate_ventouse_goldenium_avant (void)
Artiom 6:45f9cf44718a 442 {
Artiom 4:4a79942713fa 443
Artiom 6:45f9cf44718a 444 typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat;
Artiom 6:45f9cf44718a 445 static type_etat etat = init;
Artiom 6:45f9cf44718a 446
Artiom 6:45f9cf44718a 447 switch(etat) {
Artiom 6:45f9cf44718a 448 case init: //attente d'initialisation
Artiom 6:45f9cf44718a 449 if(fgoldenium_avant)
Artiom 6:45f9cf44718a 450 etat=envoi_instruction;
Artiom 6:45f9cf44718a 451 break;
Artiom 6:45f9cf44718a 452
Artiom 6:45f9cf44718a 453 case envoi_instruction://envoi instruction
Artiom 6:45f9cf44718a 454 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom 6:45f9cf44718a 455 goldenium_avant();
Artiom 14:b1abd2f6df75 456 char pompe=AV_CENTRE;
Artiom 14:b1abd2f6df75 457 can.write(CANMessage(HACHEUR_GET_ATOM, &pompe,1));
Artiom 20:42f8ec726ac8 458 SendRawId(HACHEUR_STATUT_VENTOUSES);
Artiom 6:45f9cf44718a 459 etat = attente_ack_ventouse;
Artiom 6:45f9cf44718a 460 break;
Artiom 6:45f9cf44718a 461
Artiom 20:42f8ec726ac8 462 case attente_ack_ventouse:
Artiom 6:45f9cf44718a 463 if((status_pompe&MASK_GOLDENIUM_AV)== MASK_GOLDENIUM_AV) {
Artiom 6:45f9cf44718a 464 fgoldenium_avant=0;
Artiom 6:45f9cf44718a 465 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom 6:45f9cf44718a 466 etat = init;
Artiom 6:45f9cf44718a 467 }
Artiom 6:45f9cf44718a 468 break;
Artiom 6:45f9cf44718a 469
Artiom 6:45f9cf44718a 470 }
Artiom 6:45f9cf44718a 471 }
Artiom 6:45f9cf44718a 472
Artiom 6:45f9cf44718a 473 void automate_ventouse_goldenium_arriere (void)
Artiom 6:45f9cf44718a 474 {
Artiom 6:45f9cf44718a 475
Artiom 6:45f9cf44718a 476 typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat;
Artiom 6:45f9cf44718a 477 static type_etat etat = init;
Artiom 6:45f9cf44718a 478
Artiom 6:45f9cf44718a 479 switch(etat) {
Artiom 6:45f9cf44718a 480 case init: //attente d'initialisation
Artiom 6:45f9cf44718a 481 if(fgoldenium_arriere)
Artiom 6:45f9cf44718a 482 etat=envoi_instruction;
Artiom 6:45f9cf44718a 483 break;
Artiom 6:45f9cf44718a 484
Artiom 6:45f9cf44718a 485 case envoi_instruction://envoi instruction
Artiom 6:45f9cf44718a 486 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom 6:45f9cf44718a 487 goldenium_arriere();
Artiom 14:b1abd2f6df75 488 char pompe=AR_CENTRE;
Artiom 14:b1abd2f6df75 489 can.write(CANMessage(HACHEUR_GET_ATOM, &pompe,1));
Artiom 20:42f8ec726ac8 490 SendRawId(HACHEUR_STATUT_VENTOUSES);
Artiom 6:45f9cf44718a 491 etat = attente_ack_ventouse;
Artiom 6:45f9cf44718a 492 break;
Artiom 6:45f9cf44718a 493
Artiom 20:42f8ec726ac8 494 case attente_ack_ventouse:
Artiom 6:45f9cf44718a 495 if((status_pompe&MASK_GOLDENIUM_AR)== MASK_GOLDENIUM_AR) {
Artiom 6:45f9cf44718a 496 fgoldenium_arriere=0;
Artiom 6:45f9cf44718a 497 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom 6:45f9cf44718a 498 etat = init;
Artiom 6:45f9cf44718a 499 }
Artiom 6:45f9cf44718a 500 break;
Artiom 6:45f9cf44718a 501
Artiom 6:45f9cf44718a 502 }
Artiom 6:45f9cf44718a 503
Artiom 6:45f9cf44718a 504
Artiom 6:45f9cf44718a 505 }
Artiom 6:45f9cf44718a 506
Artiom 6:45f9cf44718a 507 void automate_ventouse_sol_avant (void)
Artiom 6:45f9cf44718a 508 {
Artiom 6:45f9cf44718a 509
Artiom 6:45f9cf44718a 510 typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat;
Artiom 6:45f9cf44718a 511 static type_etat etat = init;
Artiom 6:45f9cf44718a 512
Artiom 6:45f9cf44718a 513 switch(etat) {
Artiom 6:45f9cf44718a 514 case init: //attente d'initialisation
Artiom 6:45f9cf44718a 515 if(fsol_avant)
Artiom 6:45f9cf44718a 516 etat=envoi_instruction;
Artiom 6:45f9cf44718a 517 break;
Artiom 6:45f9cf44718a 518
Artiom 6:45f9cf44718a 519 case envoi_instruction://envoi instruction
Artiom 6:45f9cf44718a 520 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom 6:45f9cf44718a 521 sol_avant_baisser();
Artiom 13:9e19048eb065 522 char pompe=AV_BAS;
Artiom 13:9e19048eb065 523 can.write(CANMessage(HACHEUR_GET_ATOM, &pompe,1));
Artiom 6:45f9cf44718a 524 SendRawId(HACHEUR_STATUT_VENTOUSES);
Artiom 6:45f9cf44718a 525 etat = attente_ack_ventouse;
Artiom 6:45f9cf44718a 526 break;
Artiom 6:45f9cf44718a 527
Artiom 6:45f9cf44718a 528 case attente_ack_ventouse:
Artiom 6:45f9cf44718a 529 if((status_pompe&MASK_SOL_AV)== MASK_SOL_AV) {
Artiom 6:45f9cf44718a 530 sol_avant_remonter();
Artiom 6:45f9cf44718a 531 fsol_avant=0;
Artiom 6:45f9cf44718a 532 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom 6:45f9cf44718a 533 etat = init;
Artiom 6:45f9cf44718a 534 }
Artiom 6:45f9cf44718a 535 break;
Artiom 6:45f9cf44718a 536
Artiom 6:45f9cf44718a 537 }
Artiom 6:45f9cf44718a 538
Artiom 6:45f9cf44718a 539
Artiom 6:45f9cf44718a 540 }
Artiom 6:45f9cf44718a 541 void automate_ventouse_sol_arriere (void)
Artiom 6:45f9cf44718a 542 {
Artiom 6:45f9cf44718a 543
Artiom 6:45f9cf44718a 544 typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat;
Artiom 6:45f9cf44718a 545 static type_etat etat = init;
Artiom 6:45f9cf44718a 546
Artiom 6:45f9cf44718a 547 switch(etat) {
Artiom 6:45f9cf44718a 548 case init: //attente d'initialisation
Artiom 6:45f9cf44718a 549 if(fsol_arriere)
Artiom 6:45f9cf44718a 550 etat=envoi_instruction;
Artiom 6:45f9cf44718a 551 break;
Artiom 6:45f9cf44718a 552
Artiom 6:45f9cf44718a 553 case envoi_instruction://envoi instruction
Artiom 6:45f9cf44718a 554 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom 6:45f9cf44718a 555 sol_arriere_baisser();
Artiom 13:9e19048eb065 556 char pompe=AR_BAS;
Artiom 13:9e19048eb065 557 can.write(CANMessage(HACHEUR_GET_ATOM, &pompe,1));
Artiom 6:45f9cf44718a 558 SendRawId(HACHEUR_STATUT_VENTOUSES);
Artiom 6:45f9cf44718a 559 etat = attente_ack_ventouse;
Artiom 6:45f9cf44718a 560 break;
Artiom 6:45f9cf44718a 561
Artiom 6:45f9cf44718a 562 case attente_ack_ventouse:
Artiom 6:45f9cf44718a 563 if((status_pompe&MASK_SOL_AR)== MASK_SOL_AR) {
Artiom 6:45f9cf44718a 564 sol_arriere_remonter();
Artiom 6:45f9cf44718a 565 fsol_arriere=0;
Artiom 6:45f9cf44718a 566 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom 6:45f9cf44718a 567 etat = init;
Artiom 6:45f9cf44718a 568 }
Artiom 6:45f9cf44718a 569 break;
Artiom 6:45f9cf44718a 570
Artiom 6:45f9cf44718a 571 }
Artiom 4:4a79942713fa 572
Artiom 4:4a79942713fa 573
Artiom 11:6c11c081c855 574 }
Artiom 11:6c11c081c855 575 void automate_ventouse_balance_avant (void)
Artiom 11:6c11c081c855 576 {
Artiom 11:6c11c081c855 577 typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat;
Artiom 11:6c11c081c855 578 static type_etat etat = init;
Artiom 11:6c11c081c855 579
Artiom 11:6c11c081c855 580 switch(etat) {
Artiom 11:6c11c081c855 581 case init: //attente d'initialisation
Artiom 11:6c11c081c855 582 if(fbalance_avant)
Artiom 11:6c11c081c855 583 etat=envoi_instruction;
Artiom 11:6c11c081c855 584 break;
Artiom 11:6c11c081c855 585
Artiom 11:6c11c081c855 586 case envoi_instruction://envoi instruction
Artiom 11:6c11c081c855 587 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom 15:119fc452dcaf 588 presentoir_avant();
Artiom 20:42f8ec726ac8 589 verification();
Artiom 12:191486ba5118 590 balance_avant_placement();
Artiom 12:191486ba5118 591 Rotate(200);
Artiom 12:191486ba5118 592 wait(0.5);
Artiom 11:6c11c081c855 593 balance_avant();
Artiom 12:191486ba5118 594 Rotate(-200);
Artiom 12:191486ba5118 595 wait(0.5);
Artiom 20:42f8ec726ac8 596 SendRawId(HACHEUR_STATUT_VENTOUSES);
Artiom 11:6c11c081c855 597 etat = attente_ack_ventouse;
Artiom 11:6c11c081c855 598 break;
Artiom 11:6c11c081c855 599
Artiom 11:6c11c081c855 600 case attente_ack_ventouse:
Artiom 12:191486ba5118 601 char pompe=AV_DROIT;
Artiom 12:191486ba5118 602 can.write(CANMessage(HACHEUR_RELEASE_ATOM, &pompe,1));
Artiom 11:6c11c081c855 603 if((status_pompe&MASK_AV_DROIT)== 0) {
Artiom 11:6c11c081c855 604 fbalance_avant=0;
Artiom 12:191486ba5118 605 presentoir_arriere();
Artiom 12:191486ba5118 606 presentoir_avant();
Artiom 12:191486ba5118 607 can.write(CANMessage(HACHEUR_RELEASE_ATOM, &pompe,1));//on arrete la pompe car elle est réactivé par presentoir_avant();
Artiom 12:191486ba5118 608 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom 12:191486ba5118 609 etat = init;
Artiom 12:191486ba5118 610 }
Artiom 12:191486ba5118 611 break;
Artiom 12:191486ba5118 612
Artiom 12:191486ba5118 613 }
Artiom 12:191486ba5118 614
Artiom 12:191486ba5118 615
Artiom 12:191486ba5118 616 }
Artiom 12:191486ba5118 617 void automate_ventouse_balance_arriere (void)
Artiom 12:191486ba5118 618 {
Artiom 12:191486ba5118 619 typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat;
Artiom 12:191486ba5118 620 static type_etat etat = init;
Artiom 12:191486ba5118 621
Artiom 12:191486ba5118 622 switch(etat) {
Artiom 12:191486ba5118 623 case init: //attente d'initialisation
Artiom 12:191486ba5118 624 if(fbalance_arriere)
Artiom 12:191486ba5118 625 etat=envoi_instruction;
Artiom 12:191486ba5118 626 break;
Artiom 12:191486ba5118 627
Artiom 12:191486ba5118 628 case envoi_instruction://envoi instruction
Artiom 12:191486ba5118 629 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom 15:119fc452dcaf 630 presentoir_arriere();
Artiom 20:42f8ec726ac8 631 verification();
Artiom 12:191486ba5118 632 balance_arriere_placement();
Artiom 12:191486ba5118 633 Rotate(200);
Artiom 12:191486ba5118 634 wait(0.5);
Artiom 12:191486ba5118 635 balance_arriere();
Artiom 12:191486ba5118 636 Rotate(-200);
Artiom 12:191486ba5118 637 wait(0.5);
Artiom 16:2a66cfe37262 638 SendRawId(HACHEUR_STATUT_VENTOUSES);
Artiom 12:191486ba5118 639 etat = attente_ack_ventouse;
Artiom 12:191486ba5118 640 break;
Artiom 12:191486ba5118 641
Artiom 12:191486ba5118 642 case attente_ack_ventouse:
Artiom 12:191486ba5118 643 char pompe=AR_DROIT;
Artiom 12:191486ba5118 644 can.write(CANMessage(HACHEUR_RELEASE_ATOM, &pompe,1));
Artiom 12:191486ba5118 645 if((status_pompe&MASK_AR_DROIT)== 0) {
Artiom 12:191486ba5118 646 fbalance_arriere=0;
Artiom 12:191486ba5118 647 presentoir_arriere();
Artiom 12:191486ba5118 648 presentoir_avant();
Artiom 12:191486ba5118 649 can.write(CANMessage(HACHEUR_RELEASE_ATOM, &pompe,1));//on arrete la pompe car elle est réactivé par presentoir_avant();
Artiom 12:191486ba5118 650 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom 12:191486ba5118 651 etat = init;
Artiom 12:191486ba5118 652 }
Artiom 12:191486ba5118 653 break;
Artiom 12:191486ba5118 654
Artiom 12:191486ba5118 655 }
Artiom 12:191486ba5118 656
Artiom 12:191486ba5118 657
Artiom 12:191486ba5118 658 }
Artiom 13:9e19048eb065 659 void automate_ventouse_relache_avant (void)
Artiom 13:9e19048eb065 660 {
Artiom 13:9e19048eb065 661 typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat;
Artiom 13:9e19048eb065 662 static type_etat etat = init;
Artiom 13:9e19048eb065 663
Artiom 13:9e19048eb065 664 switch(etat) {
Artiom 13:9e19048eb065 665 case init: //attente d'initialisation
Artiom 13:9e19048eb065 666 if(favant_relache)
Artiom 13:9e19048eb065 667 etat=envoi_instruction;
Artiom 13:9e19048eb065 668 break;
Artiom 13:9e19048eb065 669
Artiom 13:9e19048eb065 670 case envoi_instruction://envoi instruction
Artiom 13:9e19048eb065 671 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom 16:2a66cfe37262 672 SendRawId(HACHEUR_STATUT_VENTOUSES);
Artiom 13:9e19048eb065 673 etat = attente_ack_ventouse;
Artiom 13:9e19048eb065 674 break;
Artiom 13:9e19048eb065 675
Artiom 13:9e19048eb065 676 case attente_ack_ventouse:
Artiom 13:9e19048eb065 677 SendRawId(HACHEUR_RELEASE_AV);
Artiom 13:9e19048eb065 678 if((status_pompe&MASK_PRESENTOIR_AV)== 0) {
Artiom 13:9e19048eb065 679 favant_relache=0;
Artiom 13:9e19048eb065 680 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom 13:9e19048eb065 681 etat = init;
Artiom 13:9e19048eb065 682 }
Artiom 13:9e19048eb065 683 break;
Artiom 13:9e19048eb065 684
Artiom 13:9e19048eb065 685 }
Artiom 13:9e19048eb065 686
Artiom 13:9e19048eb065 687
Artiom 13:9e19048eb065 688 }
Artiom 12:191486ba5118 689 void automate_ventouse_relache_arriere (void)
Artiom 12:191486ba5118 690 {
Artiom 12:191486ba5118 691 typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat;
Artiom 12:191486ba5118 692 static type_etat etat = init;
Artiom 12:191486ba5118 693
Artiom 12:191486ba5118 694 switch(etat) {
Artiom 12:191486ba5118 695 case init: //attente d'initialisation
Artiom 12:191486ba5118 696 if(farriere_relache)
Artiom 12:191486ba5118 697 etat=envoi_instruction;
Artiom 12:191486ba5118 698 break;
Artiom 12:191486ba5118 699
Artiom 12:191486ba5118 700 case envoi_instruction://envoi instruction
Artiom 12:191486ba5118 701 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom 16:2a66cfe37262 702 SendRawId(HACHEUR_STATUT_VENTOUSES);
Artiom 12:191486ba5118 703 etat = attente_ack_ventouse;
Artiom 12:191486ba5118 704 break;
Artiom 12:191486ba5118 705
Artiom 12:191486ba5118 706 case attente_ack_ventouse:
Artiom 20:42f8ec726ac8 707 SendRawId(HACHEUR_RELEASE_AR);
Artiom 12:191486ba5118 708 if((status_pompe&MASK_PRESENTOIR_AR)== 0) {
Artiom 12:191486ba5118 709 farriere_relache=0;
Artiom 11:6c11c081c855 710 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom 11:6c11c081c855 711 etat = init;
Artiom 11:6c11c081c855 712 }
Artiom 11:6c11c081c855 713 break;
Artiom 11:6c11c081c855 714 }
Artiom 11:6c11c081c855 715
Artiom 13:9e19048eb065 716 }
Artiom 13:9e19048eb065 717 void automate_ventouse_sol_avant_relache (void)
Artiom 13:9e19048eb065 718 {
Artiom 13:9e19048eb065 719 typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat;
Artiom 13:9e19048eb065 720 static type_etat etat = init;
Artiom 13:9e19048eb065 721
Artiom 13:9e19048eb065 722 switch(etat) {
Artiom 13:9e19048eb065 723 case init: //attente d'initialisation
Artiom 13:9e19048eb065 724 if(fsol_avant_relache)
Artiom 13:9e19048eb065 725 etat=envoi_instruction;
Artiom 13:9e19048eb065 726 break;
Artiom 13:9e19048eb065 727
Artiom 13:9e19048eb065 728 case envoi_instruction://envoi instruction
Artiom 13:9e19048eb065 729 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom 13:9e19048eb065 730 positionControl(AV_sol,520,1,BLED_ON,4);//baisser
Artiom 13:9e19048eb065 731 verification();
Artiom 16:2a66cfe37262 732 SendRawId(HACHEUR_STATUT_VENTOUSES);
Artiom 13:9e19048eb065 733 etat = attente_ack_ventouse;
Artiom 13:9e19048eb065 734 break;
Artiom 13:9e19048eb065 735
Artiom 13:9e19048eb065 736 case attente_ack_ventouse:
Artiom 13:9e19048eb065 737 char pompe=AV_BAS;
Artiom 13:9e19048eb065 738 can.write(CANMessage(HACHEUR_RELEASE_ATOM, &pompe,1));
Artiom 13:9e19048eb065 739 if((status_pompe&MASK_SOL_AV)== 0) {
Artiom 13:9e19048eb065 740 sol_avant_remonter();
Artiom 13:9e19048eb065 741 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom 13:9e19048eb065 742 fsol_avant_relache=0;
Artiom 13:9e19048eb065 743 etat = init;
Artiom 13:9e19048eb065 744 }
Artiom 13:9e19048eb065 745 break;
Artiom 13:9e19048eb065 746
Artiom 13:9e19048eb065 747 }
Artiom 13:9e19048eb065 748
Artiom 13:9e19048eb065 749
Artiom 13:9e19048eb065 750 }
Artiom 13:9e19048eb065 751
Artiom 13:9e19048eb065 752 void automate_ventouse_sol_arriere_relache (void)
Artiom 13:9e19048eb065 753 {
Artiom 13:9e19048eb065 754 typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat;
Artiom 13:9e19048eb065 755 static type_etat etat = init;
Artiom 13:9e19048eb065 756
Artiom 13:9e19048eb065 757 switch(etat) {
Artiom 13:9e19048eb065 758 case init: //attente d'initialisation
Artiom 13:9e19048eb065 759 if(fsol_arriere_relache)
Artiom 13:9e19048eb065 760 etat=envoi_instruction;
Artiom 13:9e19048eb065 761 break;
Artiom 13:9e19048eb065 762
Artiom 13:9e19048eb065 763 case envoi_instruction://envoi instruction
Artiom 13:9e19048eb065 764 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom 13:9e19048eb065 765 positionControl(AR_sol,480,1,BLED_ON,4);//baisser
Artiom 13:9e19048eb065 766 verification();
Artiom 20:42f8ec726ac8 767 SendRawId(HACHEUR_STATUT_VENTOUSES);
Artiom 13:9e19048eb065 768 etat = attente_ack_ventouse;
Artiom 13:9e19048eb065 769 break;
Artiom 13:9e19048eb065 770
Artiom 13:9e19048eb065 771 case attente_ack_ventouse:
Artiom 13:9e19048eb065 772 char pompe=AR_BAS;
Artiom 13:9e19048eb065 773 can.write(CANMessage(HACHEUR_RELEASE_ATOM, &pompe,1));
Artiom 13:9e19048eb065 774 if((status_pompe&MASK_SOL_AR)== 0) {
Artiom 13:9e19048eb065 775 sol_arriere_remonter();
Artiom 13:9e19048eb065 776 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom 13:9e19048eb065 777 fsol_arriere_relache=0;
Artiom 13:9e19048eb065 778 etat = init;
Artiom 13:9e19048eb065 779 }
Artiom 13:9e19048eb065 780 break;
Artiom 13:9e19048eb065 781
Artiom 13:9e19048eb065 782 }
Artiom 13:9e19048eb065 783
Artiom 13:9e19048eb065 784
Artiom 14:b1abd2f6df75 785 }
Artiom 14:b1abd2f6df75 786 void automate_ventouse_accelerateur_avant (void)
Artiom 14:b1abd2f6df75 787 {
Artiom 16:2a66cfe37262 788 typedef enum {init,envoi_instruction,attente_ack_ventouse_droite_gauche,attente_ack_ventouse_centre} type_etat;
Artiom 14:b1abd2f6df75 789 static type_etat etat = init;
Artiom 14:b1abd2f6df75 790
Artiom 14:b1abd2f6df75 791 switch(etat) {
Artiom 14:b1abd2f6df75 792 case init: //attente d'initialisation
Artiom 14:b1abd2f6df75 793 if(faccelerateur_avant)
Artiom 14:b1abd2f6df75 794 etat=envoi_instruction;
Artiom 14:b1abd2f6df75 795 break;
Artiom 14:b1abd2f6df75 796
Artiom 14:b1abd2f6df75 797 case envoi_instruction://envoi instruction
Artiom 14:b1abd2f6df75 798 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom 14:b1abd2f6df75 799 accelerateur_avant();
Artiom 14:b1abd2f6df75 800 GoStraight(200,1,35,0);//35epaisseur accelerateur
Artiom 16:2a66cfe37262 801 SendRawId(HACHEUR_STATUT_VENTOUSES);
Artiom 16:2a66cfe37262 802 etat = attente_ack_ventouse_droite_gauche;
Artiom 14:b1abd2f6df75 803 break;
Artiom 14:b1abd2f6df75 804
Artiom 16:2a66cfe37262 805 case attente_ack_ventouse_droite_gauche:
Artiom 14:b1abd2f6df75 806 char pompe=AV_DROIT;
Artiom 14:b1abd2f6df75 807 can.write(CANMessage(HACHEUR_RELEASE_ATOM, &pompe,1));
Artiom 14:b1abd2f6df75 808 pompe=AV_GAUCHE;
Artiom 14:b1abd2f6df75 809 can.write(CANMessage(HACHEUR_RELEASE_ATOM, &pompe,1));
Artiom 16:2a66cfe37262 810 if((status_pompe&MASK_AV_DROIT_GAUCHE)== 0) {
Artiom 16:2a66cfe37262 811 GoStraight(-20,0,0,0);
Artiom 16:2a66cfe37262 812 wait(1);
Artiom 16:2a66cfe37262 813 presentoir_avant();
Artiom 16:2a66cfe37262 814 etat = attente_ack_ventouse_centre;
Artiom 16:2a66cfe37262 815 }
Artiom 16:2a66cfe37262 816 case attente_ack_ventouse_centre:
Artiom 20:42f8ec726ac8 817 pompe=AV_CENTRE;
Artiom 20:42f8ec726ac8 818 can.write(CANMessage(HACHEUR_RELEASE_ATOM, &pompe,1));
Artiom 14:b1abd2f6df75 819 if((status_pompe&MASK_AV_DROIT_GAUCHE)== 0) {
Artiom 14:b1abd2f6df75 820 faccelerateur_avant=0;
Artiom 14:b1abd2f6df75 821 presentoir_avant();
Artiom 14:b1abd2f6df75 822 can.write(CANMessage(HACHEUR_RELEASE_ATOM, &pompe,1));//on arrete la pompe car elle est réactivé par presentoir_avant();
Artiom 14:b1abd2f6df75 823 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom 14:b1abd2f6df75 824 etat = init;
Artiom 14:b1abd2f6df75 825 }
Artiom 20:42f8ec726ac8 826
Artiom 14:b1abd2f6df75 827 break;
Artiom 14:b1abd2f6df75 828
Artiom 14:b1abd2f6df75 829 }
Artiom 14:b1abd2f6df75 830
Artiom 14:b1abd2f6df75 831
Artiom 14:b1abd2f6df75 832 }
Artiom 14:b1abd2f6df75 833
Artiom 14:b1abd2f6df75 834
Artiom 14:b1abd2f6df75 835 void automate_ventouse_accelerateur_arriere (void)
Artiom 14:b1abd2f6df75 836 {
Artiom 14:b1abd2f6df75 837
Artiom 14:b1abd2f6df75 838 }
Artiom 14:b1abd2f6df75 839
Artiom 14:b1abd2f6df75 840