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