CRAC Team / Mbed 2 deprecated ACRAC_carte_esclave_GROS_ROBOT_2019

Carte esclave gros robot

Dependencies:   mbed Herkulex_Library_2019 ident_crac actions_Pr

main.cpp@11:6c11c081c855, 2019-05-20 (annotated)

Committer:
Artiom
Date:
Mon May 20 11:38:43 2019 +0000
Revision:
11:6c11c081c855
Parent:
10:48c0d1b8aed9
Child:
12:191486ba5118
ajout de la fonction verification fin de mouvement+ack ventouse relache

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 6:45f9cf44718a 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 11:6c11c081c855 23 char fbalance_avant=0;
Artiom 4:4a79942713fa 24 char status_pompe=0;
Artiom 4:4a79942713fa 25
Artiom 0:bc74da1c502f 26 void canRx_ISR (void);
Artiom 0:bc74da1c502f 27 void SendAck(unsigned short id, unsigned short from);
Artiom 0:bc74da1c502f 28 void SendRawId (unsigned short id);
Artiom 0:bc74da1c502f 29 void GoStraight (signed short distance,unsigned char recalage, unsigned short newValue, unsigned char isEnchainement);
Artiom 0:bc74da1c502f 30 void canProcessRx(void);
Artiom 6:45f9cf44718a 31
Artiom 4:4a79942713fa 32 void automate_ventouse_presentoir_avant(void);
Artiom 5:6e198cdd99ad 33 void automate_ventouse_presentoir_arriere (void);
Artiom 0:bc74da1c502f 34
Artiom 6:45f9cf44718a 35 void automate_ventouse_goldenium_avant (void);
Artiom 6:45f9cf44718a 36 void automate_ventouse_goldenium_arriere (void);
Artiom 6:45f9cf44718a 37
Artiom 6:45f9cf44718a 38 void automate_ventouse_sol_avant (void);
Artiom 6:45f9cf44718a 39 void automate_ventouse_sol_arriere(void);
Artiom 6:45f9cf44718a 40
Artiom 7:553f3f1c2c53 41 void automate_ventouse_sol_avant_relache(void);
Artiom 7:553f3f1c2c53 42 void automate_ventouse_sol_arriere_relache(void);
Artiom 11:6c11c081c855 43
Artiom 11:6c11c081c855 44 void automate_ventouse_balance_avant (void);
Artiom 0:bc74da1c502f 45 /*********************************************************************************************/
Artiom 0:bc74da1c502f 46 /* FUNCTION NAME: canRx_ISR */
Artiom 0:bc74da1c502f 47 /* DESCRIPTION : lit les messages sur le can et les stocke dans la FIFO */
Artiom 0:bc74da1c502f 48 /*********************************************************************************************/
Artiom 0:bc74da1c502f 49 void canRx_ISR (void)
Artiom 0:bc74da1c502f 50 {
Artiom 0:bc74da1c502f 51 if (can.read(msgRxBuffer[FIFO_ecriture])) {
Artiom 0:bc74da1c502f 52 if (msgRxBuffer[FIFO_ecriture].id==GLOBAL_GAME_END) {
Artiom 0:bc74da1c502f 53
Artiom 0:bc74da1c502f 54 }
Artiom 0:bc74da1c502f 55 FIFO_ecriture=(FIFO_ecriture+1)%SIZE_FIFO;
Artiom 0:bc74da1c502f 56
Artiom 0:bc74da1c502f 57 }
Artiom 0:bc74da1c502f 58
Artiom 0:bc74da1c502f 59 }
Artiom 0:bc74da1c502f 60 /*********************************************************************************************/
Artiom 0:bc74da1c502f 61 /* FUNCTION NAME: SendAck */
Artiom 0:bc74da1c502f 62 /* DESCRIPTION : */
Artiom 0:bc74da1c502f 63 /*********************************************************************************************/
Artiom 0:bc74da1c502f 64 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 65 {
Artiom 0:bc74da1c502f 66 CANMessage msgTx=CANMessage();
Artiom 0:bc74da1c502f 67 msgTx.id=from; //waitingAckFrom
Artiom 0:bc74da1c502f 68 msgTx.len=2;
Artiom 0:bc74da1c502f 69 msgTx.format=CANStandard;
Artiom 0:bc74da1c502f 70 msgTx.type=CANData;
Artiom 0:bc74da1c502f 71 // from sur 2 octets
Artiom 0:bc74da1c502f 72 msgTx.data[0]=(unsigned char)id; //waitingAckID
Artiom 0:bc74da1c502f 73 msgTx.data[1]=(unsigned char)(id>>8);
Artiom 0:bc74da1c502f 74 can.write(msgTx);
Artiom 0:bc74da1c502f 75 }
Artiom 0:bc74da1c502f 76 /*********************************************************************************************/
Artiom 0:bc74da1c502f 77 /* FUNCTION NAME: SendRawId */
Artiom 0:bc74da1c502f 78 /* DESCRIPTION : */
Artiom 0:bc74da1c502f 79 /*********************************************************************************************/
Artiom 0:bc74da1c502f 80 void SendRawId (unsigned short id)
Artiom 0:bc74da1c502f 81 {
Artiom 0:bc74da1c502f 82 CANMessage msgTx=CANMessage();
Artiom 0:bc74da1c502f 83 msgTx.id=id;
Artiom 0:bc74da1c502f 84 msgTx.len=0;
Artiom 0:bc74da1c502f 85 can.write(msgTx); //carte esclave f446re
Artiom 0:bc74da1c502f 86 wait_us(200);
Artiom 0:bc74da1c502f 87 }
Artiom 0:bc74da1c502f 88
Artiom 0:bc74da1c502f 89 /*********************************************************************************************/
Artiom 0:bc74da1c502f 90 /* FUNCTION NAME: GoStraight */
Artiom 0:bc74da1c502f 91 /* DESCRIPTION : Transmission CAN correspondant à une ligne droite, avec ou sans recalage */
Artiom 0:bc74da1c502f 92 /* recalage : 0 => pas de recalage */
Artiom 0:bc74da1c502f 93 /* 1 => recalage en X */
Artiom 0:bc74da1c502f 94 /* 2 => Recalage en Y */
Artiom 0:bc74da1c502f 95 /* newValue : Uniquement en cas de recalage, indique la nouvelle valeur de l'odo */
Artiom 0:bc74da1c502f 96 /* isEnchainement : Indique si il faut executer l'instruction en enchainement */
Artiom 0:bc74da1c502f 97 /* 0 => non */
Artiom 0:bc74da1c502f 98 /* 1 => oui */
Artiom 0:bc74da1c502f 99 /* 2 => dernière instruction de l'enchainement */
Artiom 0:bc74da1c502f 100 /*********************************************************************************************/
Artiom 0:bc74da1c502f 101 void GoStraight (signed short distance,unsigned char recalage, unsigned short newValue, unsigned char isEnchainement)
Artiom 0:bc74da1c502f 102 {
Artiom 0:bc74da1c502f 103 CANMessage msgTx=CANMessage();
Artiom 0:bc74da1c502f 104 msgTx.id=ASSERVISSEMENT_RECALAGE;
Artiom 0:bc74da1c502f 105 msgTx.len=6;
Artiom 0:bc74da1c502f 106 msgTx.format=CANStandard;
Artiom 0:bc74da1c502f 107 msgTx.type=CANData;
Artiom 0:bc74da1c502f 108 // x sur 2 octets
Artiom 0:bc74da1c502f 109 msgTx.data[0]=(unsigned char)distance;
Artiom 0:bc74da1c502f 110 msgTx.data[1]=(unsigned char)(distance>>8);
Artiom 0:bc74da1c502f 111 //Recalage sur 1 octet
Artiom 0:bc74da1c502f 112 msgTx.data[2]=recalage;
Artiom 0:bc74da1c502f 113 //Valeur du recalage sur 2 octets
Artiom 0:bc74da1c502f 114 msgTx.data[3]=(unsigned char)newValue;
Artiom 0:bc74da1c502f 115 msgTx.data[4]=(unsigned char)(newValue>>8);
Artiom 0:bc74da1c502f 116 //Enchainement sur 1 octet
Artiom 0:bc74da1c502f 117 msgTx.data[5]=isEnchainement;
Artiom 0:bc74da1c502f 118
Artiom 0:bc74da1c502f 119 can.write(msgTx);
Artiom 0:bc74da1c502f 120 //wait_ms(500);
Artiom 0:bc74da1c502f 121 }
Artiom 0:bc74da1c502f 122
Artiom 0:bc74da1c502f 123 int main()
Artiom 0:bc74da1c502f 124 {
Artiom 0:bc74da1c502f 125 can.attach(&canRx_ISR); // création de l'interrupt attachée à la réception sur le CAN
Artiom 0:bc74da1c502f 126 servo_interrupt_en(); //permettre les interuptions
Artiom 11:6c11c081c855 127
Artiom 0:bc74da1c502f 128 wait(1);//attente servo boot
Artiom 2:9e63099cca99 129 gabarit_petit_robot();
Artiom 0:bc74da1c502f 130 while(1) {
Artiom 0:bc74da1c502f 131 canProcessRx();
Artiom 10:48c0d1b8aed9 132 f_mesure();//dt35
Artiom 11:6c11c081c855 133
Artiom 4:4a79942713fa 134 automate_ventouse_presentoir_avant();
Artiom 5:6e198cdd99ad 135 automate_ventouse_presentoir_arriere();
Artiom 7:553f3f1c2c53 136
Artiom 6:45f9cf44718a 137 automate_ventouse_goldenium_avant();
Artiom 6:45f9cf44718a 138 automate_ventouse_goldenium_arriere();
Artiom 7:553f3f1c2c53 139
Artiom 6:45f9cf44718a 140 automate_ventouse_sol_avant();
Artiom 7:553f3f1c2c53 141 automate_ventouse_sol_arriere();
Artiom 7:553f3f1c2c53 142
Artiom 11:6c11c081c855 143 automate_ventouse_balance_avant();
Artiom 11:6c11c081c855 144
Artiom 0:bc74da1c502f 145 if(EtatGameEnd==1) {
Artiom 0:bc74da1c502f 146 while(1);
Artiom 0:bc74da1c502f 147 }
Artiom 0:bc74da1c502f 148
Artiom 0:bc74da1c502f 149 }
Artiom 0:bc74da1c502f 150 }
Artiom 0:bc74da1c502f 151
Artiom 0:bc74da1c502f 152 /****************************************************************************************/
Artiom 0:bc74da1c502f 153 /* FUNCTION NAME: canProcessRx */
Artiom 0:bc74da1c502f 154 /* DESCRIPTION : Fonction de traitement des messages CAN */
Artiom 0:bc74da1c502f 155 /****************************************************************************************/
Artiom 0:bc74da1c502f 156 void canProcessRx(void)
Artiom 0:bc74da1c502f 157 {
Artiom 0:bc74da1c502f 158 static signed char FIFO_occupation=0,FIFO_max_occupation=0;
Artiom 0:bc74da1c502f 159 CANMessage msgTx=CANMessage();
Artiom 0:bc74da1c502f 160 FIFO_occupation=FIFO_ecriture-FIFO_lecture;
Artiom 0:bc74da1c502f 161 if(FIFO_occupation<0)
Artiom 0:bc74da1c502f 162 FIFO_occupation=FIFO_occupation+SIZE_FIFO;
Artiom 0:bc74da1c502f 163 if(FIFO_max_occupation<FIFO_occupation)
Artiom 0:bc74da1c502f 164 FIFO_max_occupation=FIFO_occupation;
Artiom 0:bc74da1c502f 165 if(FIFO_occupation!=0) {
Artiom 0:bc74da1c502f 166 int identifiant=msgRxBuffer[FIFO_lecture].id;
Artiom 0:bc74da1c502f 167
Artiom 0:bc74da1c502f 168 switch(identifiant) {
Artiom 0:bc74da1c502f 169
Artiom 0:bc74da1c502f 170 case GLOBAL_GAME_END:
Artiom 0:bc74da1c502f 171 EtatGameEnd = 1;
Artiom 0:bc74da1c502f 172 break;
Artiom 0:bc74da1c502f 173
Artiom 0:bc74da1c502f 174 case DATA_TELEMETRE: //Lit le telemetre N°X suivant la data dans le CAN
Artiom 0:bc74da1c502f 175 char numero_telemetre=msgRxBuffer[FIFO_lecture].data[0];
Artiom 0:bc74da1c502f 176 short distance=lecture_telemetre(numero_telemetre);
Artiom 0:bc74da1c502f 177
Artiom 0:bc74da1c502f 178
Artiom 0:bc74da1c502f 179 msgTx.id=RECEPTION_DATA; // tx Valeur Telemetre1
Artiom 0:bc74da1c502f 180 msgTx.len=2;
Artiom 0:bc74da1c502f 181 msgTx.format=CANStandard;
Artiom 0:bc74da1c502f 182 msgTx.type=CANData;
Artiom 0:bc74da1c502f 183 // Rayon sur 2 octets
Artiom 0:bc74da1c502f 184 msgTx.data[0]=(unsigned char)distance;
Artiom 0:bc74da1c502f 185 msgTx.data[1]=(unsigned char)(distance>>8);
Artiom 0:bc74da1c502f 186
Artiom 0:bc74da1c502f 187 can.write(msgTx);
Artiom 0:bc74da1c502f 188 SendAck(ACKNOWLEDGE_TELEMETRE,RECEPTION_DATA);
Artiom 0:bc74da1c502f 189 break;
Artiom 0:bc74da1c502f 190
Artiom 0:bc74da1c502f 191 case DATA_RECALAGE:
Artiom 0:bc74da1c502f 192 short distance1=lecture_telemetre(1);
Artiom 0:bc74da1c502f 193 short distance2=lecture_telemetre(2);
Artiom 0:bc74da1c502f 194 short distance3=lecture_telemetre(3);
Artiom 0:bc74da1c502f 195 short distance4=lecture_telemetre(4);
Artiom 0:bc74da1c502f 196
Artiom 0:bc74da1c502f 197 msgTx.id=RECEPTION_RECALAGE; // tx Valeur Telemetre1
Artiom 0:bc74da1c502f 198 msgTx.len=8;
Artiom 0:bc74da1c502f 199 msgTx.format=CANStandard;
Artiom 0:bc74da1c502f 200 msgTx.type=CANData;
Artiom 0:bc74da1c502f 201 // Rayon sur 2 octets
Artiom 0:bc74da1c502f 202 msgTx.data[0]=(unsigned char)distance1;
Artiom 0:bc74da1c502f 203 msgTx.data[1]=(unsigned char)(distance1>>8);
Artiom 0:bc74da1c502f 204 msgTx.data[2]=(unsigned char)distance2;
Artiom 0:bc74da1c502f 205 msgTx.data[3]=(unsigned char)(distance2>>8);
Artiom 0:bc74da1c502f 206 msgTx.data[4]=(unsigned char)distance3;
Artiom 0:bc74da1c502f 207 msgTx.data[5]=(unsigned char)(distance3>>8);
Artiom 0:bc74da1c502f 208 msgTx.data[6]=(unsigned char)distance4;
Artiom 0:bc74da1c502f 209 msgTx.data[7]=(unsigned char)(distance4>>8);
Artiom 0:bc74da1c502f 210 can.write(msgTx);
Artiom 0:bc74da1c502f 211 SendAck(ACKNOWLEDGE_TELEMETRE,RECEPTION_RECALAGE);
Artiom 0:bc74da1c502f 212 break;
Artiom 0:bc74da1c502f 213
Artiom 0:bc74da1c502f 214 #ifdef ROBOT_SMALL
Artiom 4:4a79942713fa 215 //-------------------------------------------------------------------------Actions petit robot----------------------------------------------
Artiom 2:9e63099cca99 216 case GABARIT_PETIT_ROBOT:
Artiom 2:9e63099cca99 217 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom 2:9e63099cca99 218 gabarit_petit_robot();
Artiom 2:9e63099cca99 219 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom 0:bc74da1c502f 220 break;
Artiom 0:bc74da1c502f 221
Artiom 0:bc74da1c502f 222 case PRESENTOIR_AVANT:
Artiom 4:4a79942713fa 223 fpresentoir_avant=1;
Artiom 0:bc74da1c502f 224 break;
Artiom 0:bc74da1c502f 225
Artiom 0:bc74da1c502f 226 case PRESENTOIR_ARRIERE:
Artiom 5:6e198cdd99ad 227 fpresentoir_arriere=1;
Artiom 0:bc74da1c502f 228 break;
Artiom 0:bc74da1c502f 229
Artiom 0:bc74da1c502f 230 case BALANCE_AVANT:
Artiom 11:6c11c081c855 231 fbalance_avant=1;
Artiom 0:bc74da1c502f 232 break;
Artiom 0:bc74da1c502f 233
Artiom 0:bc74da1c502f 234 case BALANCE_ARRIERE:
Artiom 2:9e63099cca99 235 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom 0:bc74da1c502f 236 balance_arriere();
Artiom 2:9e63099cca99 237 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom 2:9e63099cca99 238 break;
Artiom 2:9e63099cca99 239
Artiom 2:9e63099cca99 240 case ACCELERATEUR_AVANT:
Artiom 2:9e63099cca99 241 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom 2:9e63099cca99 242 accelerateur_avant();
Artiom 2:9e63099cca99 243 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom 2:9e63099cca99 244 break;
Artiom 2:9e63099cca99 245
Artiom 2:9e63099cca99 246 case ACCELERATEUR_ARRIERE:
Artiom 2:9e63099cca99 247 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom 2:9e63099cca99 248 accelerateur_arriere();
Artiom 2:9e63099cca99 249 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom 2:9e63099cca99 250 break;
Artiom 6:45f9cf44718a 251
Artiom 2:9e63099cca99 252 case GOLDENIUM_AVANT:
Artiom 6:45f9cf44718a 253 fgoldenium_avant=1;
Artiom 2:9e63099cca99 254 break;
Artiom 2:9e63099cca99 255
Artiom 2:9e63099cca99 256 case GOLDENIUM_ARRIERE:
Artiom 6:45f9cf44718a 257 fgoldenium_arriere=1;
Artiom 2:9e63099cca99 258 break;
Artiom 6:45f9cf44718a 259
Artiom 2:9e63099cca99 260 case SOL_AVANT:
Artiom 6:45f9cf44718a 261 fsol_avant=1;
Artiom 2:9e63099cca99 262 break;
Artiom 2:9e63099cca99 263
Artiom 2:9e63099cca99 264 case SOL_ARRIERE:
Artiom 6:45f9cf44718a 265 fsol_arriere=1;
Artiom 2:9e63099cca99 266 break;
Artiom 2:9e63099cca99 267
Artiom 2:9e63099cca99 268 case SOL_AVANT_RELACHE:
Artiom 7:553f3f1c2c53 269 fsol_avant_relache=1;
Artiom 2:9e63099cca99 270 break;
Artiom 2:9e63099cca99 271
Artiom 2:9e63099cca99 272 case SOL_ARRIERE_RELACHE:
Artiom 7:553f3f1c2c53 273 fsol_arriere_relache=1;
Artiom 0:bc74da1c502f 274 break;
Artiom 4:4a79942713fa 275 //--------------------------------------------------------------------------ACK carte pompe----------------------------------------------
Artiom 4:4a79942713fa 276
Artiom 4:4a79942713fa 277
Artiom 4:4a79942713fa 278 case HACHEUR_STATUT_VENTOUSES:
Artiom 4:4a79942713fa 279 status_pompe = msgRxBuffer[FIFO_lecture].data[1];
Artiom 5:6e198cdd99ad 280 //can.write(CANMessage(0x529, &status_pompe,1));
Artiom 4:4a79942713fa 281 break;
Artiom 4:4a79942713fa 282
Artiom 4:4a79942713fa 283 case HACHEUR_GET_ATOM_ACK:
Artiom 4:4a79942713fa 284 status_pompe |= (0x01 << msgRxBuffer[FIFO_lecture].data[0]);
Artiom 5:6e198cdd99ad 285 //can.write(CANMessage(0x529, &status_pompe,1));
Artiom 4:4a79942713fa 286 break;
Artiom 4:4a79942713fa 287
Artiom 4:4a79942713fa 288 case HACHEUR_RELEASE_ATOM_ACK :
Artiom 4:4a79942713fa 289 status_pompe &= ~(0x01 << msgRxBuffer[FIFO_lecture].data[0]);
Artiom 4:4a79942713fa 290 break;
Artiom 4:4a79942713fa 291 //-------------------------------------------------------------------------------------------------------------------------------------------
Artiom 4:4a79942713fa 292
Artiom 0:bc74da1c502f 293
Artiom 0:bc74da1c502f 294 #endif
Artiom 0:bc74da1c502f 295 #ifdef ROBOT_BIG
Artiom 0:bc74da1c502f 296 case 3:
Artiom 0:bc74da1c502f 297
Artiom 0:bc74da1c502f 298 break;
Artiom 0:bc74da1c502f 299
Artiom 0:bc74da1c502f 300 case 5:
Artiom 0:bc74da1c502f 301
Artiom 0:bc74da1c502f 302 break;
Artiom 0:bc74da1c502f 303
Artiom 0:bc74da1c502f 304 #endif
Artiom 0:bc74da1c502f 305 }
Artiom 0:bc74da1c502f 306 FIFO_lecture=(FIFO_lecture+1)%SIZE_FIFO;
Artiom 0:bc74da1c502f 307
Artiom 0:bc74da1c502f 308 }
Artiom 4:4a79942713fa 309
Artiom 4:4a79942713fa 310
Artiom 4:4a79942713fa 311 }
Artiom 4:4a79942713fa 312
Artiom 4:4a79942713fa 313 void automate_ventouse_presentoir_avant (void)
Artiom 4:4a79942713fa 314 {
Artiom 4:4a79942713fa 315
Artiom 4:4a79942713fa 316 typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat;
Artiom 4:4a79942713fa 317 static type_etat etat = init;
Artiom 4:4a79942713fa 318
Artiom 4:4a79942713fa 319 switch(etat) {
Artiom 4:4a79942713fa 320 case init: //attente d'initialisation
Artiom 4:4a79942713fa 321 if(fpresentoir_avant)
Artiom 4:4a79942713fa 322 etat=envoi_instruction;
Artiom 4:4a79942713fa 323 break;
Artiom 4:4a79942713fa 324
Artiom 4:4a79942713fa 325 case envoi_instruction://envoi instruction
Artiom 4:4a79942713fa 326 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom 4:4a79942713fa 327 presentoir_avant();
Artiom 5:6e198cdd99ad 328 SendRawId(HACHEUR_STATUT_VENTOUSES);
Artiom 4:4a79942713fa 329 etat = attente_ack_ventouse;
Artiom 4:4a79942713fa 330 break;
Artiom 4:4a79942713fa 331
Artiom 4:4a79942713fa 332 case attente_ack_ventouse:
Artiom 5:6e198cdd99ad 333 if((status_pompe&MASK_PRESENTOIR_AV)==MASK_PRESENTOIR_AV) {
Artiom 4:4a79942713fa 334 fpresentoir_avant=0;
Artiom 5:6e198cdd99ad 335 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom 4:4a79942713fa 336 etat = init;
Artiom 5:6e198cdd99ad 337 }
Artiom 5:6e198cdd99ad 338 break;
Artiom 5:6e198cdd99ad 339
Artiom 5:6e198cdd99ad 340 }
Artiom 5:6e198cdd99ad 341 }
Artiom 5:6e198cdd99ad 342 void automate_ventouse_presentoir_arriere (void)
Artiom 5:6e198cdd99ad 343 {
Artiom 5:6e198cdd99ad 344
Artiom 5:6e198cdd99ad 345 typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat;
Artiom 5:6e198cdd99ad 346 static type_etat etat = init;
Artiom 5:6e198cdd99ad 347
Artiom 5:6e198cdd99ad 348 switch(etat) {
Artiom 5:6e198cdd99ad 349 case init: //attente d'initialisation
Artiom 5:6e198cdd99ad 350 if(fpresentoir_arriere)
Artiom 5:6e198cdd99ad 351 etat=envoi_instruction;
Artiom 5:6e198cdd99ad 352 break;
Artiom 5:6e198cdd99ad 353
Artiom 5:6e198cdd99ad 354 case envoi_instruction://envoi instruction
Artiom 5:6e198cdd99ad 355 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom 5:6e198cdd99ad 356 presentoir_arriere();
Artiom 5:6e198cdd99ad 357 SendRawId(HACHEUR_STATUT_VENTOUSES);
Artiom 5:6e198cdd99ad 358 etat = attente_ack_ventouse;
Artiom 5:6e198cdd99ad 359 break;
Artiom 5:6e198cdd99ad 360
Artiom 5:6e198cdd99ad 361 case attente_ack_ventouse:
Artiom 5:6e198cdd99ad 362 if((status_pompe&MASK_PRESENTOIR_AR)==MASK_PRESENTOIR_AR) {
Artiom 5:6e198cdd99ad 363 fpresentoir_arriere=0;
Artiom 5:6e198cdd99ad 364 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom 5:6e198cdd99ad 365 etat = init;
Artiom 4:4a79942713fa 366 }
Artiom 4:4a79942713fa 367 break;
Artiom 4:4a79942713fa 368
Artiom 4:4a79942713fa 369 }
Artiom 6:45f9cf44718a 370 }
Artiom 6:45f9cf44718a 371 void automate_ventouse_goldenium_avant (void)
Artiom 6:45f9cf44718a 372 {
Artiom 4:4a79942713fa 373
Artiom 6:45f9cf44718a 374 typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat;
Artiom 6:45f9cf44718a 375 static type_etat etat = init;
Artiom 6:45f9cf44718a 376
Artiom 6:45f9cf44718a 377 switch(etat) {
Artiom 6:45f9cf44718a 378 case init: //attente d'initialisation
Artiom 6:45f9cf44718a 379 if(fgoldenium_avant)
Artiom 6:45f9cf44718a 380 etat=envoi_instruction;
Artiom 6:45f9cf44718a 381 break;
Artiom 6:45f9cf44718a 382
Artiom 6:45f9cf44718a 383 case envoi_instruction://envoi instruction
Artiom 6:45f9cf44718a 384 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom 6:45f9cf44718a 385 goldenium_avant();
Artiom 6:45f9cf44718a 386 SendRawId(HACHEUR_STATUT_VENTOUSES);
Artiom 6:45f9cf44718a 387 etat = attente_ack_ventouse;
Artiom 6:45f9cf44718a 388 break;
Artiom 6:45f9cf44718a 389
Artiom 6:45f9cf44718a 390 case attente_ack_ventouse:
Artiom 6:45f9cf44718a 391 if((status_pompe&MASK_GOLDENIUM_AV)== MASK_GOLDENIUM_AV) {
Artiom 6:45f9cf44718a 392 fgoldenium_avant=0;
Artiom 6:45f9cf44718a 393 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom 6:45f9cf44718a 394 etat = init;
Artiom 6:45f9cf44718a 395 }
Artiom 6:45f9cf44718a 396 break;
Artiom 6:45f9cf44718a 397
Artiom 6:45f9cf44718a 398 }
Artiom 6:45f9cf44718a 399 }
Artiom 6:45f9cf44718a 400
Artiom 6:45f9cf44718a 401 void automate_ventouse_goldenium_arriere (void)
Artiom 6:45f9cf44718a 402 {
Artiom 6:45f9cf44718a 403
Artiom 6:45f9cf44718a 404 typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat;
Artiom 6:45f9cf44718a 405 static type_etat etat = init;
Artiom 6:45f9cf44718a 406
Artiom 6:45f9cf44718a 407 switch(etat) {
Artiom 6:45f9cf44718a 408 case init: //attente d'initialisation
Artiom 6:45f9cf44718a 409 if(fgoldenium_arriere)
Artiom 6:45f9cf44718a 410 etat=envoi_instruction;
Artiom 6:45f9cf44718a 411 break;
Artiom 6:45f9cf44718a 412
Artiom 6:45f9cf44718a 413 case envoi_instruction://envoi instruction
Artiom 6:45f9cf44718a 414 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom 6:45f9cf44718a 415 goldenium_arriere();
Artiom 6:45f9cf44718a 416 SendRawId(HACHEUR_STATUT_VENTOUSES);
Artiom 6:45f9cf44718a 417 etat = attente_ack_ventouse;
Artiom 6:45f9cf44718a 418 break;
Artiom 6:45f9cf44718a 419
Artiom 6:45f9cf44718a 420 case attente_ack_ventouse:
Artiom 6:45f9cf44718a 421 if((status_pompe&MASK_GOLDENIUM_AR)== MASK_GOLDENIUM_AR) {
Artiom 6:45f9cf44718a 422 fgoldenium_arriere=0;
Artiom 6:45f9cf44718a 423 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom 6:45f9cf44718a 424 etat = init;
Artiom 6:45f9cf44718a 425 }
Artiom 6:45f9cf44718a 426 break;
Artiom 6:45f9cf44718a 427
Artiom 6:45f9cf44718a 428 }
Artiom 6:45f9cf44718a 429
Artiom 6:45f9cf44718a 430
Artiom 6:45f9cf44718a 431 }
Artiom 6:45f9cf44718a 432
Artiom 6:45f9cf44718a 433 void automate_ventouse_sol_avant (void)
Artiom 6:45f9cf44718a 434 {
Artiom 6:45f9cf44718a 435
Artiom 6:45f9cf44718a 436 typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat;
Artiom 6:45f9cf44718a 437 static type_etat etat = init;
Artiom 6:45f9cf44718a 438
Artiom 6:45f9cf44718a 439 switch(etat) {
Artiom 6:45f9cf44718a 440 case init: //attente d'initialisation
Artiom 6:45f9cf44718a 441 if(fsol_avant)
Artiom 6:45f9cf44718a 442 etat=envoi_instruction;
Artiom 6:45f9cf44718a 443 break;
Artiom 6:45f9cf44718a 444
Artiom 6:45f9cf44718a 445 case envoi_instruction://envoi instruction
Artiom 6:45f9cf44718a 446 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom 6:45f9cf44718a 447 sol_avant_baisser();
Artiom 6:45f9cf44718a 448 SendRawId(HACHEUR_STATUT_VENTOUSES);
Artiom 6:45f9cf44718a 449 etat = attente_ack_ventouse;
Artiom 6:45f9cf44718a 450 break;
Artiom 6:45f9cf44718a 451
Artiom 6:45f9cf44718a 452 case attente_ack_ventouse:
Artiom 6:45f9cf44718a 453 if((status_pompe&MASK_SOL_AV)== MASK_SOL_AV) {
Artiom 6:45f9cf44718a 454 sol_avant_remonter();
Artiom 6:45f9cf44718a 455 fsol_avant=0;
Artiom 6:45f9cf44718a 456 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom 6:45f9cf44718a 457 etat = init;
Artiom 6:45f9cf44718a 458 }
Artiom 6:45f9cf44718a 459 break;
Artiom 6:45f9cf44718a 460
Artiom 6:45f9cf44718a 461 }
Artiom 6:45f9cf44718a 462
Artiom 6:45f9cf44718a 463
Artiom 6:45f9cf44718a 464 }
Artiom 6:45f9cf44718a 465 void automate_ventouse_sol_arriere (void)
Artiom 6:45f9cf44718a 466 {
Artiom 6:45f9cf44718a 467
Artiom 6:45f9cf44718a 468 typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat;
Artiom 6:45f9cf44718a 469 static type_etat etat = init;
Artiom 6:45f9cf44718a 470
Artiom 6:45f9cf44718a 471 switch(etat) {
Artiom 6:45f9cf44718a 472 case init: //attente d'initialisation
Artiom 6:45f9cf44718a 473 if(fsol_arriere)
Artiom 6:45f9cf44718a 474 etat=envoi_instruction;
Artiom 6:45f9cf44718a 475 break;
Artiom 6:45f9cf44718a 476
Artiom 6:45f9cf44718a 477 case envoi_instruction://envoi instruction
Artiom 6:45f9cf44718a 478 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom 6:45f9cf44718a 479 sol_arriere_baisser();
Artiom 6:45f9cf44718a 480 SendRawId(HACHEUR_STATUT_VENTOUSES);
Artiom 6:45f9cf44718a 481 etat = attente_ack_ventouse;
Artiom 6:45f9cf44718a 482 break;
Artiom 6:45f9cf44718a 483
Artiom 6:45f9cf44718a 484 case attente_ack_ventouse:
Artiom 6:45f9cf44718a 485 if((status_pompe&MASK_SOL_AR)== MASK_SOL_AR) {
Artiom 6:45f9cf44718a 486 sol_arriere_remonter();
Artiom 6:45f9cf44718a 487 fsol_arriere=0;
Artiom 6:45f9cf44718a 488 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom 6:45f9cf44718a 489 etat = init;
Artiom 6:45f9cf44718a 490 }
Artiom 6:45f9cf44718a 491 break;
Artiom 6:45f9cf44718a 492
Artiom 6:45f9cf44718a 493 }
Artiom 4:4a79942713fa 494
Artiom 4:4a79942713fa 495
Artiom 11:6c11c081c855 496 }
Artiom 11:6c11c081c855 497 void automate_ventouse_balance_avant (void)
Artiom 11:6c11c081c855 498 {
Artiom 11:6c11c081c855 499 typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat;
Artiom 11:6c11c081c855 500 static type_etat etat = init;
Artiom 11:6c11c081c855 501
Artiom 11:6c11c081c855 502 switch(etat) {
Artiom 11:6c11c081c855 503 case init: //attente d'initialisation
Artiom 11:6c11c081c855 504 if(fbalance_avant)
Artiom 11:6c11c081c855 505 etat=envoi_instruction;
Artiom 11:6c11c081c855 506 break;
Artiom 11:6c11c081c855 507
Artiom 11:6c11c081c855 508 case envoi_instruction://envoi instruction
Artiom 11:6c11c081c855 509 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom 11:6c11c081c855 510 balance_avant();
Artiom 11:6c11c081c855 511 SendRawId(HACHEUR_STATUT_VENTOUSES);
Artiom 11:6c11c081c855 512 etat = attente_ack_ventouse;
Artiom 11:6c11c081c855 513 break;
Artiom 11:6c11c081c855 514
Artiom 11:6c11c081c855 515 case attente_ack_ventouse:
Artiom 11:6c11c081c855 516 if((status_pompe&MASK_AV_DROIT)== 0) {
Artiom 11:6c11c081c855 517 fbalance_avant=0;
Artiom 11:6c11c081c855 518 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom 11:6c11c081c855 519 etat = init;
Artiom 11:6c11c081c855 520 }
Artiom 11:6c11c081c855 521 break;
Artiom 11:6c11c081c855 522
Artiom 11:6c11c081c855 523 }
Artiom 11:6c11c081c855 524
Artiom 11:6c11c081c855 525
Artiom 0:bc74da1c502f 526 }