CRAC Team / Mbed 2 deprecated carte_esclave_PETIT_ROBOT_2019

carte esclave Petit Robot

Dependencies:   mbed Herkulex_Library_2019 actions_Gr ident_crac actions_Pr

main.cpp@19:511ff570c2f5, 2019-05-21 (annotated)

Committer:
kyxstark
Date:
Tue May 21 15:34:48 2019 +0000
Revision:
19:511ff570c2f5
Parent:
18:0ea0d887966f
Parent:
17:b8ffd3593d0e
Child:
20:42f8ec726ac8
test_merge

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 13:9e19048eb065 325
Artiom 4:4a79942713fa 326 //--------------------------------------------------------------------------ACK carte pompe----------------------------------------------
Artiom 4:4a79942713fa 327
Artiom 4:4a79942713fa 328
Artiom 4:4a79942713fa 329 case HACHEUR_STATUT_VENTOUSES:
Artiom 4:4a79942713fa 330 status_pompe = msgRxBuffer[FIFO_lecture].data[1];
Artiom 5:6e198cdd99ad 331 //can.write(CANMessage(0x529, &status_pompe,1));
Artiom 4:4a79942713fa 332 break;
Artiom 4:4a79942713fa 333
Artiom 4:4a79942713fa 334 case HACHEUR_GET_ATOM_ACK:
Artiom 4:4a79942713fa 335 status_pompe |= (0x01 << msgRxBuffer[FIFO_lecture].data[0]);
Artiom 5:6e198cdd99ad 336 //can.write(CANMessage(0x529, &status_pompe,1));
Artiom 4:4a79942713fa 337 break;
Artiom 4:4a79942713fa 338
Artiom 4:4a79942713fa 339 case HACHEUR_RELEASE_ATOM_ACK :
Artiom 4:4a79942713fa 340 status_pompe &= ~(0x01 << msgRxBuffer[FIFO_lecture].data[0]);
Artiom 4:4a79942713fa 341 break;
Artiom 13:9e19048eb065 342
Artiom 13:9e19048eb065 343
Artiom 4:4a79942713fa 344 //-------------------------------------------------------------------------------------------------------------------------------------------
Artiom 4:4a79942713fa 345
Artiom 0:bc74da1c502f 346
Artiom 0:bc74da1c502f 347 #endif
Artiom 0:bc74da1c502f 348 #ifdef ROBOT_BIG
kyxstark 18:0ea0d887966f 349 case COUROIES:
kyxstark 18:0ea0d887966f 350 rotation_couroies = msgRxBuffer[FIFO_lecture].data[0];
Artiom 0:bc74da1c502f 351 break;
Artiom 0:bc74da1c502f 352
Artiom 0:bc74da1c502f 353 case 5:
Artiom 0:bc74da1c502f 354
Artiom 0:bc74da1c502f 355 break;
Artiom 0:bc74da1c502f 356
Artiom 0:bc74da1c502f 357 #endif
Artiom 0:bc74da1c502f 358 }
Artiom 0:bc74da1c502f 359 FIFO_lecture=(FIFO_lecture+1)%SIZE_FIFO;
Artiom 0:bc74da1c502f 360
Artiom 0:bc74da1c502f 361 }
Artiom 4:4a79942713fa 362
Artiom 4:4a79942713fa 363
Artiom 4:4a79942713fa 364 }
Artiom 4:4a79942713fa 365
Artiom 4:4a79942713fa 366 void automate_ventouse_presentoir_avant (void)
Artiom 4:4a79942713fa 367 {
Artiom 4:4a79942713fa 368
Artiom 4:4a79942713fa 369 typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat;
Artiom 4:4a79942713fa 370 static type_etat etat = init;
Artiom 4:4a79942713fa 371
Artiom 4:4a79942713fa 372 switch(etat) {
Artiom 4:4a79942713fa 373 case init: //attente d'initialisation
Artiom 4:4a79942713fa 374 if(fpresentoir_avant)
Artiom 4:4a79942713fa 375 etat=envoi_instruction;
Artiom 4:4a79942713fa 376 break;
Artiom 4:4a79942713fa 377
Artiom 4:4a79942713fa 378 case envoi_instruction://envoi instruction
Artiom 4:4a79942713fa 379 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom 4:4a79942713fa 380 presentoir_avant();
Artiom 5:6e198cdd99ad 381 SendRawId(HACHEUR_STATUT_VENTOUSES);
Artiom 4:4a79942713fa 382 etat = attente_ack_ventouse;
Artiom 4:4a79942713fa 383 break;
Artiom 4:4a79942713fa 384
Artiom 4:4a79942713fa 385 case attente_ack_ventouse:
Artiom 5:6e198cdd99ad 386 if((status_pompe&MASK_PRESENTOIR_AV)==MASK_PRESENTOIR_AV) {
Artiom 4:4a79942713fa 387 fpresentoir_avant=0;
Artiom 5:6e198cdd99ad 388 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom 4:4a79942713fa 389 etat = init;
Artiom 5:6e198cdd99ad 390 }
Artiom 5:6e198cdd99ad 391 break;
Artiom 5:6e198cdd99ad 392
Artiom 5:6e198cdd99ad 393 }
Artiom 5:6e198cdd99ad 394 }
Artiom 5:6e198cdd99ad 395 void automate_ventouse_presentoir_arriere (void)
Artiom 5:6e198cdd99ad 396 {
Artiom 5:6e198cdd99ad 397
Artiom 5:6e198cdd99ad 398 typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat;
Artiom 5:6e198cdd99ad 399 static type_etat etat = init;
Artiom 5:6e198cdd99ad 400
Artiom 5:6e198cdd99ad 401 switch(etat) {
Artiom 5:6e198cdd99ad 402 case init: //attente d'initialisation
Artiom 5:6e198cdd99ad 403 if(fpresentoir_arriere)
Artiom 5:6e198cdd99ad 404 etat=envoi_instruction;
Artiom 5:6e198cdd99ad 405 break;
Artiom 5:6e198cdd99ad 406
Artiom 5:6e198cdd99ad 407 case envoi_instruction://envoi instruction
Artiom 5:6e198cdd99ad 408 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom 5:6e198cdd99ad 409 presentoir_arriere();
Artiom 5:6e198cdd99ad 410 SendRawId(HACHEUR_STATUT_VENTOUSES);
Artiom 5:6e198cdd99ad 411 etat = attente_ack_ventouse;
Artiom 5:6e198cdd99ad 412 break;
Artiom 5:6e198cdd99ad 413
Artiom 5:6e198cdd99ad 414 case attente_ack_ventouse:
Artiom 5:6e198cdd99ad 415 if((status_pompe&MASK_PRESENTOIR_AR)==MASK_PRESENTOIR_AR) {
Artiom 5:6e198cdd99ad 416 fpresentoir_arriere=0;
Artiom 5:6e198cdd99ad 417 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom 5:6e198cdd99ad 418 etat = init;
Artiom 4:4a79942713fa 419 }
Artiom 4:4a79942713fa 420 break;
Artiom 4:4a79942713fa 421
Artiom 4:4a79942713fa 422 }
Artiom 6:45f9cf44718a 423 }
Artiom 6:45f9cf44718a 424 void automate_ventouse_goldenium_avant (void)
Artiom 6:45f9cf44718a 425 {
Artiom 4:4a79942713fa 426
Artiom 6:45f9cf44718a 427 typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat;
Artiom 6:45f9cf44718a 428 static type_etat etat = init;
Artiom 6:45f9cf44718a 429
Artiom 6:45f9cf44718a 430 switch(etat) {
Artiom 6:45f9cf44718a 431 case init: //attente d'initialisation
Artiom 6:45f9cf44718a 432 if(fgoldenium_avant)
Artiom 6:45f9cf44718a 433 etat=envoi_instruction;
Artiom 6:45f9cf44718a 434 break;
Artiom 6:45f9cf44718a 435
Artiom 6:45f9cf44718a 436 case envoi_instruction://envoi instruction
Artiom 6:45f9cf44718a 437 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom 6:45f9cf44718a 438 goldenium_avant();
Artiom 14:b1abd2f6df75 439 char pompe=AV_CENTRE;
Artiom 14:b1abd2f6df75 440 can.write(CANMessage(HACHEUR_GET_ATOM, &pompe,1));
Artiom 16:2a66cfe37262 441 SendRawId(HACHEUR_STATUT_VENTOUSES);
Artiom 6:45f9cf44718a 442 etat = attente_ack_ventouse;
Artiom 6:45f9cf44718a 443 break;
Artiom 6:45f9cf44718a 444
Artiom 16:2a66cfe37262 445 case attente_ack_ventouse:
Artiom 6:45f9cf44718a 446 if((status_pompe&MASK_GOLDENIUM_AV)== MASK_GOLDENIUM_AV) {
Artiom 6:45f9cf44718a 447 fgoldenium_avant=0;
Artiom 6:45f9cf44718a 448 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom 6:45f9cf44718a 449 etat = init;
Artiom 6:45f9cf44718a 450 }
Artiom 6:45f9cf44718a 451 break;
Artiom 6:45f9cf44718a 452
Artiom 6:45f9cf44718a 453 }
Artiom 6:45f9cf44718a 454 }
Artiom 6:45f9cf44718a 455
Artiom 6:45f9cf44718a 456 void automate_ventouse_goldenium_arriere (void)
Artiom 6:45f9cf44718a 457 {
Artiom 6:45f9cf44718a 458
Artiom 6:45f9cf44718a 459 typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat;
Artiom 6:45f9cf44718a 460 static type_etat etat = init;
Artiom 6:45f9cf44718a 461
Artiom 6:45f9cf44718a 462 switch(etat) {
Artiom 6:45f9cf44718a 463 case init: //attente d'initialisation
Artiom 6:45f9cf44718a 464 if(fgoldenium_arriere)
Artiom 6:45f9cf44718a 465 etat=envoi_instruction;
Artiom 6:45f9cf44718a 466 break;
Artiom 6:45f9cf44718a 467
Artiom 6:45f9cf44718a 468 case envoi_instruction://envoi instruction
Artiom 6:45f9cf44718a 469 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom 6:45f9cf44718a 470 goldenium_arriere();
Artiom 14:b1abd2f6df75 471 char pompe=AR_CENTRE;
Artiom 14:b1abd2f6df75 472 can.write(CANMessage(HACHEUR_GET_ATOM, &pompe,1));
Artiom 16:2a66cfe37262 473 SendRawId(HACHEUR_STATUT_VENTOUSES);
Artiom 6:45f9cf44718a 474 etat = attente_ack_ventouse;
Artiom 6:45f9cf44718a 475 break;
Artiom 6:45f9cf44718a 476
Artiom 16:2a66cfe37262 477 case attente_ack_ventouse:
Artiom 6:45f9cf44718a 478 if((status_pompe&MASK_GOLDENIUM_AR)== MASK_GOLDENIUM_AR) {
Artiom 6:45f9cf44718a 479 fgoldenium_arriere=0;
Artiom 6:45f9cf44718a 480 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom 6:45f9cf44718a 481 etat = init;
Artiom 6:45f9cf44718a 482 }
Artiom 6:45f9cf44718a 483 break;
Artiom 6:45f9cf44718a 484
Artiom 6:45f9cf44718a 485 }
Artiom 6:45f9cf44718a 486
Artiom 6:45f9cf44718a 487
Artiom 6:45f9cf44718a 488 }
Artiom 6:45f9cf44718a 489
Artiom 6:45f9cf44718a 490 void automate_ventouse_sol_avant (void)
Artiom 6:45f9cf44718a 491 {
Artiom 6:45f9cf44718a 492
Artiom 6:45f9cf44718a 493 typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat;
Artiom 6:45f9cf44718a 494 static type_etat etat = init;
Artiom 6:45f9cf44718a 495
Artiom 6:45f9cf44718a 496 switch(etat) {
Artiom 6:45f9cf44718a 497 case init: //attente d'initialisation
Artiom 6:45f9cf44718a 498 if(fsol_avant)
Artiom 6:45f9cf44718a 499 etat=envoi_instruction;
Artiom 6:45f9cf44718a 500 break;
Artiom 6:45f9cf44718a 501
Artiom 6:45f9cf44718a 502 case envoi_instruction://envoi instruction
Artiom 6:45f9cf44718a 503 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom 6:45f9cf44718a 504 sol_avant_baisser();
Artiom 13:9e19048eb065 505 char pompe=AV_BAS;
Artiom 13:9e19048eb065 506 can.write(CANMessage(HACHEUR_GET_ATOM, &pompe,1));
Artiom 6:45f9cf44718a 507 SendRawId(HACHEUR_STATUT_VENTOUSES);
Artiom 6:45f9cf44718a 508 etat = attente_ack_ventouse;
Artiom 6:45f9cf44718a 509 break;
Artiom 6:45f9cf44718a 510
Artiom 6:45f9cf44718a 511 case attente_ack_ventouse:
Artiom 6:45f9cf44718a 512 if((status_pompe&MASK_SOL_AV)== MASK_SOL_AV) {
Artiom 6:45f9cf44718a 513 sol_avant_remonter();
Artiom 6:45f9cf44718a 514 fsol_avant=0;
Artiom 6:45f9cf44718a 515 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom 6:45f9cf44718a 516 etat = init;
Artiom 6:45f9cf44718a 517 }
Artiom 6:45f9cf44718a 518 break;
Artiom 6:45f9cf44718a 519
Artiom 6:45f9cf44718a 520 }
Artiom 6:45f9cf44718a 521
Artiom 6:45f9cf44718a 522
Artiom 6:45f9cf44718a 523 }
Artiom 6:45f9cf44718a 524 void automate_ventouse_sol_arriere (void)
Artiom 6:45f9cf44718a 525 {
Artiom 6:45f9cf44718a 526
Artiom 6:45f9cf44718a 527 typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat;
Artiom 6:45f9cf44718a 528 static type_etat etat = init;
Artiom 6:45f9cf44718a 529
Artiom 6:45f9cf44718a 530 switch(etat) {
Artiom 6:45f9cf44718a 531 case init: //attente d'initialisation
Artiom 6:45f9cf44718a 532 if(fsol_arriere)
Artiom 6:45f9cf44718a 533 etat=envoi_instruction;
Artiom 6:45f9cf44718a 534 break;
Artiom 6:45f9cf44718a 535
Artiom 6:45f9cf44718a 536 case envoi_instruction://envoi instruction
Artiom 6:45f9cf44718a 537 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom 6:45f9cf44718a 538 sol_arriere_baisser();
Artiom 13:9e19048eb065 539 char pompe=AR_BAS;
Artiom 13:9e19048eb065 540 can.write(CANMessage(HACHEUR_GET_ATOM, &pompe,1));
Artiom 6:45f9cf44718a 541 SendRawId(HACHEUR_STATUT_VENTOUSES);
Artiom 6:45f9cf44718a 542 etat = attente_ack_ventouse;
Artiom 6:45f9cf44718a 543 break;
Artiom 6:45f9cf44718a 544
Artiom 6:45f9cf44718a 545 case attente_ack_ventouse:
Artiom 6:45f9cf44718a 546 if((status_pompe&MASK_SOL_AR)== MASK_SOL_AR) {
Artiom 6:45f9cf44718a 547 sol_arriere_remonter();
Artiom 6:45f9cf44718a 548 fsol_arriere=0;
Artiom 6:45f9cf44718a 549 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom 6:45f9cf44718a 550 etat = init;
Artiom 6:45f9cf44718a 551 }
Artiom 6:45f9cf44718a 552 break;
Artiom 6:45f9cf44718a 553
Artiom 6:45f9cf44718a 554 }
Artiom 4:4a79942713fa 555
Artiom 4:4a79942713fa 556
Artiom 11:6c11c081c855 557 }
Artiom 11:6c11c081c855 558 void automate_ventouse_balance_avant (void)
Artiom 11:6c11c081c855 559 {
Artiom 11:6c11c081c855 560 typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat;
Artiom 11:6c11c081c855 561 static type_etat etat = init;
Artiom 11:6c11c081c855 562
Artiom 11:6c11c081c855 563 switch(etat) {
Artiom 11:6c11c081c855 564 case init: //attente d'initialisation
Artiom 11:6c11c081c855 565 if(fbalance_avant)
Artiom 11:6c11c081c855 566 etat=envoi_instruction;
Artiom 11:6c11c081c855 567 break;
Artiom 11:6c11c081c855 568
Artiom 11:6c11c081c855 569 case envoi_instruction://envoi instruction
Artiom 11:6c11c081c855 570 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom 15:119fc452dcaf 571 presentoir_avant();
Artiom 12:191486ba5118 572 balance_avant_placement();
Artiom 12:191486ba5118 573 Rotate(200);
Artiom 12:191486ba5118 574 wait(0.5);
Artiom 11:6c11c081c855 575 balance_avant();
Artiom 12:191486ba5118 576 Rotate(-200);
Artiom 12:191486ba5118 577 wait(0.5);
Artiom 16:2a66cfe37262 578 SendRawId(HACHEUR_STATUT_VENTOUSES);
Artiom 11:6c11c081c855 579 etat = attente_ack_ventouse;
Artiom 11:6c11c081c855 580 break;
Artiom 11:6c11c081c855 581
Artiom 11:6c11c081c855 582 case attente_ack_ventouse:
Artiom 12:191486ba5118 583 char pompe=AV_DROIT;
Artiom 12:191486ba5118 584 can.write(CANMessage(HACHEUR_RELEASE_ATOM, &pompe,1));
Artiom 11:6c11c081c855 585 if((status_pompe&MASK_AV_DROIT)== 0) {
Artiom 11:6c11c081c855 586 fbalance_avant=0;
Artiom 12:191486ba5118 587 presentoir_arriere();
Artiom 12:191486ba5118 588 presentoir_avant();
Artiom 12:191486ba5118 589 can.write(CANMessage(HACHEUR_RELEASE_ATOM, &pompe,1));//on arrete la pompe car elle est réactivé par presentoir_avant();
Artiom 12:191486ba5118 590 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom 12:191486ba5118 591 etat = init;
Artiom 12:191486ba5118 592 }
Artiom 12:191486ba5118 593 break;
Artiom 12:191486ba5118 594
Artiom 12:191486ba5118 595 }
Artiom 12:191486ba5118 596
Artiom 12:191486ba5118 597
Artiom 12:191486ba5118 598 }
Artiom 12:191486ba5118 599 void automate_ventouse_balance_arriere (void)
Artiom 12:191486ba5118 600 {
Artiom 12:191486ba5118 601 typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat;
Artiom 12:191486ba5118 602 static type_etat etat = init;
Artiom 12:191486ba5118 603
Artiom 12:191486ba5118 604 switch(etat) {
Artiom 12:191486ba5118 605 case init: //attente d'initialisation
Artiom 12:191486ba5118 606 if(fbalance_arriere)
Artiom 12:191486ba5118 607 etat=envoi_instruction;
Artiom 12:191486ba5118 608 break;
Artiom 12:191486ba5118 609
Artiom 12:191486ba5118 610 case envoi_instruction://envoi instruction
Artiom 12:191486ba5118 611 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom 15:119fc452dcaf 612 presentoir_arriere();
Artiom 12:191486ba5118 613 balance_arriere_placement();
Artiom 12:191486ba5118 614 Rotate(200);
Artiom 12:191486ba5118 615 wait(0.5);
Artiom 12:191486ba5118 616 balance_arriere();
Artiom 12:191486ba5118 617 Rotate(-200);
Artiom 12:191486ba5118 618 wait(0.5);
Artiom 16:2a66cfe37262 619 SendRawId(HACHEUR_STATUT_VENTOUSES);
Artiom 12:191486ba5118 620 etat = attente_ack_ventouse;
Artiom 12:191486ba5118 621 break;
Artiom 12:191486ba5118 622
Artiom 12:191486ba5118 623 case attente_ack_ventouse:
Artiom 12:191486ba5118 624 char pompe=AR_DROIT;
Artiom 12:191486ba5118 625 can.write(CANMessage(HACHEUR_RELEASE_ATOM, &pompe,1));
Artiom 12:191486ba5118 626 if((status_pompe&MASK_AR_DROIT)== 0) {
Artiom 12:191486ba5118 627 fbalance_arriere=0;
Artiom 12:191486ba5118 628 presentoir_arriere();
Artiom 12:191486ba5118 629 presentoir_avant();
Artiom 12:191486ba5118 630 can.write(CANMessage(HACHEUR_RELEASE_ATOM, &pompe,1));//on arrete la pompe car elle est réactivé par presentoir_avant();
Artiom 12:191486ba5118 631 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom 12:191486ba5118 632 etat = init;
Artiom 12:191486ba5118 633 }
Artiom 12:191486ba5118 634 break;
Artiom 12:191486ba5118 635
Artiom 12:191486ba5118 636 }
Artiom 12:191486ba5118 637
Artiom 12:191486ba5118 638
Artiom 12:191486ba5118 639 }
Artiom 13:9e19048eb065 640 void automate_ventouse_relache_avant (void)
Artiom 13:9e19048eb065 641 {
Artiom 13:9e19048eb065 642 typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat;
Artiom 13:9e19048eb065 643 static type_etat etat = init;
Artiom 13:9e19048eb065 644
Artiom 13:9e19048eb065 645 switch(etat) {
Artiom 13:9e19048eb065 646 case init: //attente d'initialisation
Artiom 13:9e19048eb065 647 if(favant_relache)
Artiom 13:9e19048eb065 648 etat=envoi_instruction;
Artiom 13:9e19048eb065 649 break;
Artiom 13:9e19048eb065 650
Artiom 13:9e19048eb065 651 case envoi_instruction://envoi instruction
Artiom 13:9e19048eb065 652 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom 16:2a66cfe37262 653 SendRawId(HACHEUR_STATUT_VENTOUSES);
Artiom 13:9e19048eb065 654 etat = attente_ack_ventouse;
Artiom 13:9e19048eb065 655 break;
Artiom 13:9e19048eb065 656
Artiom 13:9e19048eb065 657 case attente_ack_ventouse:
Artiom 13:9e19048eb065 658 SendRawId(HACHEUR_RELEASE_AV);
Artiom 13:9e19048eb065 659 if((status_pompe&MASK_PRESENTOIR_AV)== 0) {
Artiom 13:9e19048eb065 660 favant_relache=0;
Artiom 13:9e19048eb065 661 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom 13:9e19048eb065 662 etat = init;
Artiom 13:9e19048eb065 663 }
Artiom 13:9e19048eb065 664 break;
Artiom 13:9e19048eb065 665
Artiom 13:9e19048eb065 666 }
Artiom 13:9e19048eb065 667
Artiom 13:9e19048eb065 668
Artiom 13:9e19048eb065 669 }
Artiom 12:191486ba5118 670 void automate_ventouse_relache_arriere (void)
Artiom 12:191486ba5118 671 {
Artiom 12:191486ba5118 672 typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat;
Artiom 12:191486ba5118 673 static type_etat etat = init;
Artiom 12:191486ba5118 674
Artiom 12:191486ba5118 675 switch(etat) {
Artiom 12:191486ba5118 676 case init: //attente d'initialisation
Artiom 12:191486ba5118 677 if(farriere_relache)
Artiom 12:191486ba5118 678 etat=envoi_instruction;
Artiom 12:191486ba5118 679 break;
Artiom 12:191486ba5118 680
Artiom 12:191486ba5118 681 case envoi_instruction://envoi instruction
Artiom 12:191486ba5118 682 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom 16:2a66cfe37262 683 SendRawId(HACHEUR_STATUT_VENTOUSES);
Artiom 12:191486ba5118 684 etat = attente_ack_ventouse;
Artiom 12:191486ba5118 685 break;
Artiom 12:191486ba5118 686
Artiom 12:191486ba5118 687 case attente_ack_ventouse:
Artiom 16:2a66cfe37262 688 SendRawId(HACHEUR_RELEASE_AR);
Artiom 12:191486ba5118 689 if((status_pompe&MASK_PRESENTOIR_AR)== 0) {
Artiom 12:191486ba5118 690 farriere_relache=0;
Artiom 11:6c11c081c855 691 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom 11:6c11c081c855 692 etat = init;
Artiom 11:6c11c081c855 693 }
Artiom 11:6c11c081c855 694 break;
Artiom 11:6c11c081c855 695 }
Artiom 11:6c11c081c855 696
Artiom 13:9e19048eb065 697 }
Artiom 13:9e19048eb065 698 void automate_ventouse_sol_avant_relache (void)
Artiom 13:9e19048eb065 699 {
Artiom 13:9e19048eb065 700 typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat;
Artiom 13:9e19048eb065 701 static type_etat etat = init;
Artiom 13:9e19048eb065 702
Artiom 13:9e19048eb065 703 switch(etat) {
Artiom 13:9e19048eb065 704 case init: //attente d'initialisation
Artiom 13:9e19048eb065 705 if(fsol_avant_relache)
Artiom 13:9e19048eb065 706 etat=envoi_instruction;
Artiom 13:9e19048eb065 707 break;
Artiom 13:9e19048eb065 708
Artiom 13:9e19048eb065 709 case envoi_instruction://envoi instruction
Artiom 13:9e19048eb065 710 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom 13:9e19048eb065 711 positionControl(AV_sol,520,1,BLED_ON,4);//baisser
Artiom 13:9e19048eb065 712 verification();
Artiom 16:2a66cfe37262 713 SendRawId(HACHEUR_STATUT_VENTOUSES);
Artiom 13:9e19048eb065 714 etat = attente_ack_ventouse;
Artiom 13:9e19048eb065 715 break;
Artiom 13:9e19048eb065 716
Artiom 13:9e19048eb065 717 case attente_ack_ventouse:
Artiom 13:9e19048eb065 718 char pompe=AV_BAS;
Artiom 13:9e19048eb065 719 can.write(CANMessage(HACHEUR_RELEASE_ATOM, &pompe,1));
Artiom 13:9e19048eb065 720 if((status_pompe&MASK_SOL_AV)== 0) {
Artiom 13:9e19048eb065 721 sol_avant_remonter();
Artiom 13:9e19048eb065 722 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom 13:9e19048eb065 723 fsol_avant_relache=0;
Artiom 13:9e19048eb065 724 etat = init;
Artiom 13:9e19048eb065 725 }
Artiom 13:9e19048eb065 726 break;
Artiom 13:9e19048eb065 727
Artiom 13:9e19048eb065 728 }
Artiom 13:9e19048eb065 729
Artiom 13:9e19048eb065 730
Artiom 13:9e19048eb065 731 }
Artiom 13:9e19048eb065 732
Artiom 13:9e19048eb065 733 void automate_ventouse_sol_arriere_relache (void)
Artiom 13:9e19048eb065 734 {
Artiom 13:9e19048eb065 735 typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat;
Artiom 13:9e19048eb065 736 static type_etat etat = init;
Artiom 13:9e19048eb065 737
Artiom 13:9e19048eb065 738 switch(etat) {
Artiom 13:9e19048eb065 739 case init: //attente d'initialisation
Artiom 13:9e19048eb065 740 if(fsol_arriere_relache)
Artiom 13:9e19048eb065 741 etat=envoi_instruction;
Artiom 13:9e19048eb065 742 break;
Artiom 13:9e19048eb065 743
Artiom 13:9e19048eb065 744 case envoi_instruction://envoi instruction
Artiom 13:9e19048eb065 745 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom 13:9e19048eb065 746 positionControl(AR_sol,480,1,BLED_ON,4);//baisser
Artiom 13:9e19048eb065 747 verification();
Artiom 16:2a66cfe37262 748 SendRawId(HACHEUR_STATUT_VENTOUSES);
Artiom 13:9e19048eb065 749 etat = attente_ack_ventouse;
Artiom 13:9e19048eb065 750 break;
Artiom 13:9e19048eb065 751
Artiom 13:9e19048eb065 752 case attente_ack_ventouse:
Artiom 13:9e19048eb065 753 char pompe=AR_BAS;
Artiom 13:9e19048eb065 754 can.write(CANMessage(HACHEUR_RELEASE_ATOM, &pompe,1));
Artiom 13:9e19048eb065 755 if((status_pompe&MASK_SOL_AR)== 0) {
Artiom 13:9e19048eb065 756 sol_arriere_remonter();
Artiom 13:9e19048eb065 757 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom 13:9e19048eb065 758 fsol_arriere_relache=0;
Artiom 13:9e19048eb065 759 etat = init;
Artiom 13:9e19048eb065 760 }
Artiom 13:9e19048eb065 761 break;
Artiom 13:9e19048eb065 762
Artiom 13:9e19048eb065 763 }
Artiom 13:9e19048eb065 764
Artiom 13:9e19048eb065 765
Artiom 14:b1abd2f6df75 766 }
Artiom 14:b1abd2f6df75 767 void automate_ventouse_accelerateur_avant (void)
Artiom 14:b1abd2f6df75 768 {
Artiom 16:2a66cfe37262 769 typedef enum {init,envoi_instruction,attente_ack_ventouse_droite_gauche,attente_ack_ventouse_centre} type_etat;
Artiom 14:b1abd2f6df75 770 static type_etat etat = init;
Artiom 14:b1abd2f6df75 771
Artiom 14:b1abd2f6df75 772 switch(etat) {
Artiom 14:b1abd2f6df75 773 case init: //attente d'initialisation
Artiom 14:b1abd2f6df75 774 if(faccelerateur_avant)
Artiom 14:b1abd2f6df75 775 etat=envoi_instruction;
Artiom 14:b1abd2f6df75 776 break;
Artiom 14:b1abd2f6df75 777
Artiom 14:b1abd2f6df75 778 case envoi_instruction://envoi instruction
Artiom 14:b1abd2f6df75 779 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom 14:b1abd2f6df75 780 accelerateur_avant();
Artiom 14:b1abd2f6df75 781 GoStraight(200,1,35,0);//35epaisseur accelerateur
Artiom 16:2a66cfe37262 782 SendRawId(HACHEUR_STATUT_VENTOUSES);
Artiom 16:2a66cfe37262 783 etat = attente_ack_ventouse_droite_gauche;
Artiom 14:b1abd2f6df75 784 break;
Artiom 14:b1abd2f6df75 785
Artiom 16:2a66cfe37262 786 case attente_ack_ventouse_droite_gauche:
Artiom 14:b1abd2f6df75 787 char pompe=AV_DROIT;
Artiom 14:b1abd2f6df75 788 can.write(CANMessage(HACHEUR_RELEASE_ATOM, &pompe,1));
Artiom 14:b1abd2f6df75 789 pompe=AV_GAUCHE;
Artiom 14:b1abd2f6df75 790 can.write(CANMessage(HACHEUR_RELEASE_ATOM, &pompe,1));
Artiom 16:2a66cfe37262 791 if((status_pompe&MASK_AV_DROIT_GAUCHE)== 0) {
Artiom 16:2a66cfe37262 792 GoStraight(-20,0,0,0);
Artiom 16:2a66cfe37262 793 wait(1);
Artiom 16:2a66cfe37262 794 presentoir_avant();
Artiom 16:2a66cfe37262 795 etat = attente_ack_ventouse_centre;
Artiom 16:2a66cfe37262 796 }
Artiom 16:2a66cfe37262 797 case attente_ack_ventouse_centre:
Artiom 16:2a66cfe37262 798 pompe=AV_CENTRE;
Artiom 16:2a66cfe37262 799 can.write(CANMessage(HACHEUR_RELEASE_ATOM, &pompe,1));
Artiom 14:b1abd2f6df75 800 if((status_pompe&MASK_AV_DROIT_GAUCHE)== 0) {
Artiom 14:b1abd2f6df75 801 faccelerateur_avant=0;
Artiom 14:b1abd2f6df75 802 presentoir_avant();
Artiom 14:b1abd2f6df75 803 can.write(CANMessage(HACHEUR_RELEASE_ATOM, &pompe,1));//on arrete la pompe car elle est réactivé par presentoir_avant();
Artiom 14:b1abd2f6df75 804 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom 14:b1abd2f6df75 805 etat = init;
Artiom 14:b1abd2f6df75 806 }
Artiom 16:2a66cfe37262 807
Artiom 14:b1abd2f6df75 808 break;
Artiom 14:b1abd2f6df75 809
Artiom 14:b1abd2f6df75 810 }
Artiom 14:b1abd2f6df75 811
Artiom 14:b1abd2f6df75 812
Artiom 14:b1abd2f6df75 813 }
Artiom 14:b1abd2f6df75 814
Artiom 14:b1abd2f6df75 815
Artiom 14:b1abd2f6df75 816 void automate_ventouse_accelerateur_arriere (void)
Artiom 14:b1abd2f6df75 817 {
Artiom 17:b8ffd3593d0e 818 int test_merge = 0;
Artiom 14:b1abd2f6df75 819
Artiom 14:b1abd2f6df75 820 }
Artiom 14:b1abd2f6df75 821
Artiom 14:b1abd2f6df75 822