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@13:9e19048eb065, 2019-05-21 (annotated)

Committer:
Artiom
Date:
Tue May 21 08:56:49 2019 +0000
Revision:
13:9e19048eb065
Parent:
12:191486ba5118
Child:
14:b1abd2f6df75
automate relache solavant/arriere

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