CRAC Team / Mbed 2 deprecated carte_esclave2019

carte esclave petit robot

Dependencies:   mbed Herkulex_Library_2019 actions_Gr ident_crac actions_Pr

main.cpp@12:191486ba5118, 2019-05-20 (annotated)

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