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