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@6:45f9cf44718a, 2019-05-14 (annotated)

Committer:
Artiom
Date:
Tue May 14 15:41:32 2019 +0000
Revision:
6:45f9cf44718a
Parent:
5:6e198cdd99ad
Child:
7:553f3f1c2c53
ack ventouse presentoir/goldenium/sol

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