CRAC Team / Mbed 2 deprecated carte_esclave_PETIT_ROBOT_2019

carte esclave Petit Robot

Dependencies:   mbed Herkulex_Library_2019 actions_Gr ident_crac actions_Pr

main.cpp@21:edf6d03fdb20, 2019-05-22 (annotated)

Committer:
kyxstark
Date:
Wed May 22 11:19:27 2019 +0000
Revision:
21:edf6d03fdb20
Parent:
20:42f8ec726ac8
Child:
22:9e92a2b941ba
merge PR/GR;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Artiom 0:bc74da1c502f 1 #include "mbed.h"
Artiom 1:568955af8c2b 2 #include "main.h"
Artiom 0:bc74da1c502f 3
Artiom 0:bc74da1c502f 4 #define SIZE_FIFO 50
Artiom 0:bc74da1c502f 5
Artiom 0:bc74da1c502f 6 CAN can(PB_8,PB_9,1000000); // Rx&Tx pour le CAN
Artiom 0:bc74da1c502f 7
Artiom 0:bc74da1c502f 8
Artiom 0:bc74da1c502f 9 CANMessage msgRxBuffer[SIZE_FIFO];
Artiom 0:bc74da1c502f 10 unsigned char FIFO_ecriture=0; //Position du fifo pour la reception CAN
Artiom 0:bc74da1c502f 11 signed char FIFO_lecture=0;//Position du fifo de lecture des messages CAN
Artiom 0:bc74da1c502f 12 unsigned char EtatGameEnd=0;
Artiom 0:bc74da1c502f 13
kyxstark 21:edf6d03fdb20 14
kyxstark 21:edf6d03fdb20 15 int rotation_couroies = 0;
kyxstark 21:edf6d03fdb20 16
kyxstark 21:edf6d03fdb20 17
kyxstark 21:edf6d03fdb20 18 void canProcessRx(void);
Artiom 12:191486ba5118 19
Artiom 12:191486ba5118 20
Artiom 13:9e19048eb065 21
Artiom 4:4a79942713fa 22
kyxstark 21:edf6d03fdb20 23 #ifdef ROBOT_BIG
kyxstark 21:edf6d03fdb20 24 #endif
Artiom 14:b1abd2f6df75 25
Artiom 0:bc74da1c502f 26 /*********************************************************************************************/
Artiom 0:bc74da1c502f 27 /* FUNCTION NAME: canRx_ISR */
Artiom 0:bc74da1c502f 28 /* DESCRIPTION : lit les messages sur le can et les stocke dans la FIFO */
Artiom 0:bc74da1c502f 29 /*********************************************************************************************/
Artiom 0:bc74da1c502f 30 void canRx_ISR (void)
Artiom 0:bc74da1c502f 31 {
Artiom 0:bc74da1c502f 32 if (can.read(msgRxBuffer[FIFO_ecriture])) {
Artiom 0:bc74da1c502f 33 if (msgRxBuffer[FIFO_ecriture].id==GLOBAL_GAME_END) {
Artiom 0:bc74da1c502f 34
Artiom 0:bc74da1c502f 35 }
Artiom 0:bc74da1c502f 36 FIFO_ecriture=(FIFO_ecriture+1)%SIZE_FIFO;
Artiom 0:bc74da1c502f 37
Artiom 0:bc74da1c502f 38 }
Artiom 0:bc74da1c502f 39
Artiom 0:bc74da1c502f 40 }
Artiom 0:bc74da1c502f 41
Artiom 0:bc74da1c502f 42 int main()
Artiom 0:bc74da1c502f 43 {
Artiom 0:bc74da1c502f 44 can.attach(&canRx_ISR); // création de l'interrupt attachée à la réception sur le CAN
Artiom 0:bc74da1c502f 45 servo_interrupt_en(); //permettre les interuptions
Artiom 11:6c11c081c855 46
Artiom 0:bc74da1c502f 47 wait(1);//attente servo boot
kyxstark 21:edf6d03fdb20 48
kyxstark 21:edf6d03fdb20 49
kyxstark 21:edf6d03fdb20 50 #ifdef ROBOT_SMALL
Artiom 2:9e63099cca99 51 gabarit_petit_robot();
kyxstark 21:edf6d03fdb20 52 #endif
kyxstark 21:edf6d03fdb20 53
kyxstark 21:edf6d03fdb20 54
kyxstark 21:edf6d03fdb20 55
kyxstark 21:edf6d03fdb20 56 #ifdef ROBOT_BIG
kyxstark 21:edf6d03fdb20 57 clear(doigt,2);
kyxstark 21:edf6d03fdb20 58 setTorque(doigt, TORQUE_ON,2);
kyxstark 21:edf6d03fdb20 59 positionControl(doigt,384,100,BLED_ON,2);
kyxstark 21:edf6d03fdb20 60 wait(1.0);
kyxstark 21:edf6d03fdb20 61 #endif
kyxstark 21:edf6d03fdb20 62
kyxstark 21:edf6d03fdb20 63
Artiom 0:bc74da1c502f 64 while(1) {
Artiom 0:bc74da1c502f 65 canProcessRx();
Artiom 10:48c0d1b8aed9 66 f_mesure();//dt35
Artiom 11:6c11c081c855 67
kyxstark 21:edf6d03fdb20 68
kyxstark 21:edf6d03fdb20 69 #ifdef ROBOT_SMALL
Artiom 4:4a79942713fa 70 automate_ventouse_presentoir_avant();
Artiom 5:6e198cdd99ad 71 automate_ventouse_presentoir_arriere();
Artiom 7:553f3f1c2c53 72
Artiom 6:45f9cf44718a 73 automate_ventouse_goldenium_avant();
Artiom 6:45f9cf44718a 74 automate_ventouse_goldenium_arriere();
Artiom 7:553f3f1c2c53 75
Artiom 6:45f9cf44718a 76 automate_ventouse_sol_avant();
Artiom 7:553f3f1c2c53 77 automate_ventouse_sol_arriere();
Artiom 7:553f3f1c2c53 78
Artiom 11:6c11c081c855 79 automate_ventouse_balance_avant();
Artiom 12:191486ba5118 80 automate_ventouse_balance_arriere();
Artiom 12:191486ba5118 81
Artiom 13:9e19048eb065 82 automate_ventouse_relache_avant();
Artiom 12:191486ba5118 83 automate_ventouse_relache_arriere();
Artiom 14:b1abd2f6df75 84
Artiom 13:9e19048eb065 85 automate_ventouse_sol_avant_relache();
Artiom 14:b1abd2f6df75 86 automate_ventouse_sol_arriere_relache();
Artiom 14:b1abd2f6df75 87
Artiom 14:b1abd2f6df75 88 automate_ventouse_accelerateur_avant();
Artiom 14:b1abd2f6df75 89 automate_ventouse_accelerateur_arriere();
kyxstark 21:edf6d03fdb20 90 #endif
kyxstark 21:edf6d03fdb20 91
kyxstark 21:edf6d03fdb20 92 #ifdef ROBOT_BIG
kyxstark 21:edf6d03fdb20 93 fifo_couleur();
kyxstark 21:edf6d03fdb20 94 ascenseur();
kyxstark 21:edf6d03fdb20 95
kyxstark 21:edf6d03fdb20 96 #endif
Artiom 13:9e19048eb065 97
Artiom 0:bc74da1c502f 98 if(EtatGameEnd==1) {
Artiom 0:bc74da1c502f 99 while(1);
Artiom 0:bc74da1c502f 100 }
Artiom 0:bc74da1c502f 101
Artiom 0:bc74da1c502f 102 }
Artiom 0:bc74da1c502f 103 }
Artiom 0:bc74da1c502f 104
Artiom 0:bc74da1c502f 105 /****************************************************************************************/
Artiom 0:bc74da1c502f 106 /* FUNCTION NAME: canProcessRx */
Artiom 0:bc74da1c502f 107 /* DESCRIPTION : Fonction de traitement des messages CAN */
Artiom 0:bc74da1c502f 108 /****************************************************************************************/
Artiom 0:bc74da1c502f 109 void canProcessRx(void)
Artiom 0:bc74da1c502f 110 {
Artiom 0:bc74da1c502f 111 static signed char FIFO_occupation=0,FIFO_max_occupation=0;
Artiom 0:bc74da1c502f 112 CANMessage msgTx=CANMessage();
Artiom 0:bc74da1c502f 113 FIFO_occupation=FIFO_ecriture-FIFO_lecture;
Artiom 0:bc74da1c502f 114 if(FIFO_occupation<0)
Artiom 0:bc74da1c502f 115 FIFO_occupation=FIFO_occupation+SIZE_FIFO;
Artiom 0:bc74da1c502f 116 if(FIFO_max_occupation<FIFO_occupation)
Artiom 0:bc74da1c502f 117 FIFO_max_occupation=FIFO_occupation;
Artiom 0:bc74da1c502f 118 if(FIFO_occupation!=0) {
Artiom 0:bc74da1c502f 119 int identifiant=msgRxBuffer[FIFO_lecture].id;
Artiom 0:bc74da1c502f 120
Artiom 0:bc74da1c502f 121 switch(identifiant) {
Artiom 0:bc74da1c502f 122
Artiom 0:bc74da1c502f 123 case GLOBAL_GAME_END:
Artiom 0:bc74da1c502f 124 EtatGameEnd = 1;
Artiom 0:bc74da1c502f 125 break;
Artiom 0:bc74da1c502f 126
Artiom 0:bc74da1c502f 127 case DATA_TELEMETRE: //Lit le telemetre N°X suivant la data dans le CAN
Artiom 0:bc74da1c502f 128 char numero_telemetre=msgRxBuffer[FIFO_lecture].data[0];
Artiom 0:bc74da1c502f 129 short distance=lecture_telemetre(numero_telemetre);
Artiom 0:bc74da1c502f 130
Artiom 0:bc74da1c502f 131
Artiom 0:bc74da1c502f 132 msgTx.id=RECEPTION_DATA; // tx Valeur Telemetre1
Artiom 0:bc74da1c502f 133 msgTx.len=2;
Artiom 0:bc74da1c502f 134 msgTx.format=CANStandard;
Artiom 0:bc74da1c502f 135 msgTx.type=CANData;
Artiom 0:bc74da1c502f 136 // Rayon sur 2 octets
Artiom 0:bc74da1c502f 137 msgTx.data[0]=(unsigned char)distance;
Artiom 0:bc74da1c502f 138 msgTx.data[1]=(unsigned char)(distance>>8);
Artiom 0:bc74da1c502f 139
Artiom 0:bc74da1c502f 140 can.write(msgTx);
Artiom 0:bc74da1c502f 141 SendAck(ACKNOWLEDGE_TELEMETRE,RECEPTION_DATA);
Artiom 0:bc74da1c502f 142 break;
Artiom 0:bc74da1c502f 143
Artiom 0:bc74da1c502f 144 case DATA_RECALAGE:
Artiom 0:bc74da1c502f 145 short distance1=lecture_telemetre(1);
Artiom 0:bc74da1c502f 146 short distance2=lecture_telemetre(2);
Artiom 0:bc74da1c502f 147 short distance3=lecture_telemetre(3);
Artiom 0:bc74da1c502f 148 short distance4=lecture_telemetre(4);
Artiom 0:bc74da1c502f 149
Artiom 0:bc74da1c502f 150 msgTx.id=RECEPTION_RECALAGE; // tx Valeur Telemetre1
Artiom 0:bc74da1c502f 151 msgTx.len=8;
Artiom 0:bc74da1c502f 152 msgTx.format=CANStandard;
Artiom 0:bc74da1c502f 153 msgTx.type=CANData;
Artiom 0:bc74da1c502f 154 // Rayon sur 2 octets
Artiom 0:bc74da1c502f 155 msgTx.data[0]=(unsigned char)distance1;
Artiom 0:bc74da1c502f 156 msgTx.data[1]=(unsigned char)(distance1>>8);
Artiom 0:bc74da1c502f 157 msgTx.data[2]=(unsigned char)distance2;
Artiom 0:bc74da1c502f 158 msgTx.data[3]=(unsigned char)(distance2>>8);
Artiom 0:bc74da1c502f 159 msgTx.data[4]=(unsigned char)distance3;
Artiom 0:bc74da1c502f 160 msgTx.data[5]=(unsigned char)(distance3>>8);
Artiom 0:bc74da1c502f 161 msgTx.data[6]=(unsigned char)distance4;
Artiom 0:bc74da1c502f 162 msgTx.data[7]=(unsigned char)(distance4>>8);
Artiom 0:bc74da1c502f 163 can.write(msgTx);
Artiom 0:bc74da1c502f 164 SendAck(ACKNOWLEDGE_TELEMETRE,RECEPTION_RECALAGE);
Artiom 0:bc74da1c502f 165 break;
Artiom 0:bc74da1c502f 166
Artiom 0:bc74da1c502f 167 #ifdef ROBOT_SMALL
Artiom 4:4a79942713fa 168 //-------------------------------------------------------------------------Actions petit robot----------------------------------------------
Artiom 2:9e63099cca99 169 case GABARIT_PETIT_ROBOT:
Artiom 2:9e63099cca99 170 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom 2:9e63099cca99 171 gabarit_petit_robot();
Artiom 2:9e63099cca99 172 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom 0:bc74da1c502f 173 break;
Artiom 0:bc74da1c502f 174
Artiom 0:bc74da1c502f 175 case PRESENTOIR_AVANT:
Artiom 4:4a79942713fa 176 fpresentoir_avant=1;
Artiom 0:bc74da1c502f 177 break;
Artiom 0:bc74da1c502f 178
Artiom 0:bc74da1c502f 179 case PRESENTOIR_ARRIERE:
Artiom 5:6e198cdd99ad 180 fpresentoir_arriere=1;
Artiom 0:bc74da1c502f 181 break;
Artiom 0:bc74da1c502f 182
Artiom 0:bc74da1c502f 183 case BALANCE_AVANT:
Artiom 11:6c11c081c855 184 fbalance_avant=1;
Artiom 0:bc74da1c502f 185 break;
Artiom 0:bc74da1c502f 186
Artiom 0:bc74da1c502f 187 case BALANCE_ARRIERE:
Artiom 12:191486ba5118 188 fbalance_arriere=1;
Artiom 2:9e63099cca99 189 break;
Artiom 2:9e63099cca99 190
Artiom 2:9e63099cca99 191 case ACCELERATEUR_AVANT:
Artiom 14:b1abd2f6df75 192 faccelerateur_avant=1;
Artiom 2:9e63099cca99 193 break;
Artiom 2:9e63099cca99 194
Artiom 2:9e63099cca99 195 case ACCELERATEUR_ARRIERE:
Artiom 14:b1abd2f6df75 196 faccelerateur_arriere=1;
Artiom 2:9e63099cca99 197 break;
Artiom 6:45f9cf44718a 198
Artiom 2:9e63099cca99 199 case GOLDENIUM_AVANT:
Artiom 6:45f9cf44718a 200 fgoldenium_avant=1;
Artiom 2:9e63099cca99 201 break;
Artiom 2:9e63099cca99 202
Artiom 2:9e63099cca99 203 case GOLDENIUM_ARRIERE:
Artiom 6:45f9cf44718a 204 fgoldenium_arriere=1;
Artiom 2:9e63099cca99 205 break;
Artiom 6:45f9cf44718a 206
Artiom 2:9e63099cca99 207 case SOL_AVANT:
Artiom 6:45f9cf44718a 208 fsol_avant=1;
Artiom 2:9e63099cca99 209 break;
Artiom 2:9e63099cca99 210
Artiom 2:9e63099cca99 211 case SOL_ARRIERE:
Artiom 6:45f9cf44718a 212 fsol_arriere=1;
Artiom 2:9e63099cca99 213 break;
Artiom 2:9e63099cca99 214
Artiom 2:9e63099cca99 215 case SOL_AVANT_RELACHE:
Artiom 7:553f3f1c2c53 216 fsol_avant_relache=1;
Artiom 2:9e63099cca99 217 break;
Artiom 2:9e63099cca99 218
Artiom 2:9e63099cca99 219 case SOL_ARRIERE_RELACHE:
Artiom 7:553f3f1c2c53 220 fsol_arriere_relache=1;
Artiom 0:bc74da1c502f 221 break;
Artiom 12:191486ba5118 222 case AVANT_RELACHE:
Artiom 12:191486ba5118 223 favant_relache=1;
Artiom 12:191486ba5118 224 break;
Artiom 12:191486ba5118 225
Artiom 12:191486ba5118 226 case ARRIERE_RELACHE:
Artiom 12:191486ba5118 227 farriere_relache=1;
Artiom 12:191486ba5118 228 break;
Artiom 14:b1abd2f6df75 229 case RECROQUEVILLER:
Artiom 14:b1abd2f6df75 230 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom 14:b1abd2f6df75 231 recroqueviller_avant();
Artiom 14:b1abd2f6df75 232 recroqueviller_arriere();
Artiom 14:b1abd2f6df75 233 verification();
Artiom 14:b1abd2f6df75 234 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom 14:b1abd2f6df75 235 break;
Artiom 13:9e19048eb065 236
Artiom 20:42f8ec726ac8 237 case VENTOUSE_AV_CENTRE_BALANCE:
Artiom 20:42f8ec726ac8 238 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom 20:42f8ec726ac8 239 goldenium_avant();
Artiom 20:42f8ec726ac8 240 positionControl(AV_poigne_D, 270,1,BLED_ON,1);//actionneur
Artiom 20:42f8ec726ac8 241 verification();
Artiom 20:42f8ec726ac8 242 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom 20:42f8ec726ac8 243 break;
Artiom 20:42f8ec726ac8 244
Artiom 20:42f8ec726ac8 245 case VENTOUSE_AR_CENTRE_BALANCE:
Artiom 20:42f8ec726ac8 246 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom 20:42f8ec726ac8 247 goldenium_arriere();
Artiom 20:42f8ec726ac8 248 positionControl(AR_poigne_D, 200,1,BLED_ON,3);//actionneur
Artiom 20:42f8ec726ac8 249 verification();
Artiom 20:42f8ec726ac8 250 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom 20:42f8ec726ac8 251 break;
Artiom 13:9e19048eb065 252
Artiom 4:4a79942713fa 253 //--------------------------------------------------------------------------ACK carte pompe----------------------------------------------
Artiom 4:4a79942713fa 254
Artiom 4:4a79942713fa 255
Artiom 4:4a79942713fa 256 case HACHEUR_STATUT_VENTOUSES:
Artiom 4:4a79942713fa 257 status_pompe = msgRxBuffer[FIFO_lecture].data[1];
Artiom 5:6e198cdd99ad 258 //can.write(CANMessage(0x529, &status_pompe,1));
Artiom 4:4a79942713fa 259 break;
Artiom 4:4a79942713fa 260
Artiom 4:4a79942713fa 261 case HACHEUR_GET_ATOM_ACK:
Artiom 4:4a79942713fa 262 status_pompe |= (0x01 << msgRxBuffer[FIFO_lecture].data[0]);
Artiom 5:6e198cdd99ad 263 //can.write(CANMessage(0x529, &status_pompe,1));
Artiom 4:4a79942713fa 264 break;
Artiom 4:4a79942713fa 265
Artiom 4:4a79942713fa 266 case HACHEUR_RELEASE_ATOM_ACK :
Artiom 4:4a79942713fa 267 status_pompe &= ~(0x01 << msgRxBuffer[FIFO_lecture].data[0]);
Artiom 4:4a79942713fa 268 break;
Artiom 13:9e19048eb065 269
Artiom 13:9e19048eb065 270
Artiom 4:4a79942713fa 271 //-------------------------------------------------------------------------------------------------------------------------------------------
Artiom 4:4a79942713fa 272
Artiom 0:bc74da1c502f 273
Artiom 0:bc74da1c502f 274 #endif
Artiom 0:bc74da1c502f 275 #ifdef ROBOT_BIG
kyxstark 21:edf6d03fdb20 276 case ASCENSEUR:
kyxstark 18:0ea0d887966f 277 rotation_couroies = msgRxBuffer[FIFO_lecture].data[0];
Artiom 0:bc74da1c502f 278 break;
Artiom 0:bc74da1c502f 279
Artiom 0:bc74da1c502f 280 case 5:
Artiom 0:bc74da1c502f 281
Artiom 0:bc74da1c502f 282 break;
Artiom 0:bc74da1c502f 283
Artiom 0:bc74da1c502f 284 #endif
Artiom 0:bc74da1c502f 285 }
Artiom 0:bc74da1c502f 286 FIFO_lecture=(FIFO_lecture+1)%SIZE_FIFO;
Artiom 0:bc74da1c502f 287
Artiom 0:bc74da1c502f 288 }
Artiom 4:4a79942713fa 289
Artiom 4:4a79942713fa 290
Artiom 4:4a79942713fa 291 }
Artiom 4:4a79942713fa 292
Artiom 4:4a79942713fa 293
Artiom 4:4a79942713fa 294
Artiom 11:6c11c081c855 295