CRAC Team / Mbed 2 deprecated ACRAC_carte_esclave_GROS_ROBOT_2019

Carte esclave gros robot

Dependencies:   mbed Herkulex_Library_2019 ident_crac actions_Pr

main.cpp@47:ced7758fb2ce, 2020-03-12 (annotated)

Committer:
maximilienlv
Date:
Thu Mar 12 06:40:56 2020 +0000
Revision:
47:ced7758fb2ce
Parent:
46:67c0d8d971b0
Child:
48:ef0dc0df8c77
optimisation et suppression de fonctions inutiles;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Artiom 1:568955af8c2b 1 #include "main.h"
Artiom 0:bc74da1c502f 2
Artiom 0:bc74da1c502f 3 #define SIZE_FIFO 50
Artiom 0:bc74da1c502f 4
Artiom 30:d034f469359e 5
Artiom 0:bc74da1c502f 6 CAN can(PB_8,PB_9,1000000); // Rx&Tx pour le CAN
Artiom 0:bc74da1c502f 7
kyxstark 22:9e92a2b941ba 8 Serial pc(USBTX,USBRX);
Artiom 0:bc74da1c502f 9
Artiom 0:bc74da1c502f 10 CANMessage msgRxBuffer[SIZE_FIFO];
Artiom 0:bc74da1c502f 11 unsigned char FIFO_ecriture=0; //Position du fifo pour la reception CAN
Artiom 0:bc74da1c502f 12 signed char FIFO_lecture=0;//Position du fifo de lecture des messages CAN
marwanesaich 45:11614fc23e53 13 unsigned char EtatGameEnd=0, EtatGameStart = 0, EtatGameRecalage = 0;
marwanesaich 45:11614fc23e53 14 unsigned short ackFinAction = 0;
maximilienlv 47:ced7758fb2ce 15 char bras_choix=0;
Artiom 32:e9947815c4d7 16
Artiom 32:e9947815c4d7 17
marwanesaich 45:11614fc23e53 18
kyxstark 21:edf6d03fdb20 19 void canProcessRx(void);
Artiom 12:191486ba5118 20
Artiom 12:191486ba5118 21
Artiom 0:bc74da1c502f 22 /*********************************************************************************************/
Artiom 0:bc74da1c502f 23 /* FUNCTION NAME: canRx_ISR */
Artiom 0:bc74da1c502f 24 /* DESCRIPTION : lit les messages sur le can et les stocke dans la FIFO */
Artiom 0:bc74da1c502f 25 /*********************************************************************************************/
Artiom 0:bc74da1c502f 26 void canRx_ISR (void)
Artiom 0:bc74da1c502f 27 {
maximilienlv 47:ced7758fb2ce 28 if (can.read(msgRxBuffer[FIFO_ecriture]))
maximilienlv 47:ced7758fb2ce 29 {
maximilienlv 47:ced7758fb2ce 30 FIFO_ecriture=(FIFO_ecriture+1)%SIZE_FIFO;
Artiom 0:bc74da1c502f 31 }
Artiom 0:bc74da1c502f 32 }
Artiom 0:bc74da1c502f 33
Artiom 0:bc74da1c502f 34 int main()
Artiom 0:bc74da1c502f 35 {
Artiom 0:bc74da1c502f 36 can.attach(&canRx_ISR); // création de l'interrupt attachée à la réception sur le CAN
Artiom 0:bc74da1c502f 37 servo_interrupt_en(); //permettre les interuptions
maximilienlv 47:ced7758fb2ce 38 wait_ms(200);
Artiom 32:e9947815c4d7 39 deverouillage_torque();
maximilienlv 47:ced7758fb2ce 40 pc.printf("\nLAUNCHED\n\r");
maximilienlv 47:ced7758fb2ce 41 //on rentre tous les bras dans le robot dans le vagin de sa mere
kyxstark 38:9295a9721acb 42 gabarit_robot();
maximilienlv 47:ced7758fb2ce 43
maximilienlv 47:ced7758fb2ce 44 while(1)
maximilienlv 47:ced7758fb2ce 45 {
Artiom 0:bc74da1c502f 46 canProcessRx();
Artiom 10:48c0d1b8aed9 47 f_mesure();//dt35
maximilienlv 47:ced7758fb2ce 48 //aut_gabarit_robot();
marwanesaich 45:11614fc23e53 49 }
Artiom 0:bc74da1c502f 50 }
maximilienlv 47:ced7758fb2ce 51 //fin du main
Artiom 0:bc74da1c502f 52
Artiom 0:bc74da1c502f 53 /****************************************************************************************/
Artiom 0:bc74da1c502f 54 /* FUNCTION NAME: canProcessRx */
Artiom 0:bc74da1c502f 55 /* DESCRIPTION : Fonction de traitement des messages CAN */
Artiom 0:bc74da1c502f 56 /****************************************************************************************/
Artiom 0:bc74da1c502f 57 void canProcessRx(void)
Artiom 0:bc74da1c502f 58 {
Artiom 0:bc74da1c502f 59 static signed char FIFO_occupation=0,FIFO_max_occupation=0;
Artiom 0:bc74da1c502f 60 CANMessage msgTx=CANMessage();
maximilienlv 47:ced7758fb2ce 61
Artiom 0:bc74da1c502f 62 FIFO_occupation=FIFO_ecriture-FIFO_lecture;
Artiom 0:bc74da1c502f 63 if(FIFO_occupation<0)
Artiom 0:bc74da1c502f 64 FIFO_occupation=FIFO_occupation+SIZE_FIFO;
maximilienlv 47:ced7758fb2ce 65
Artiom 0:bc74da1c502f 66 if(FIFO_max_occupation<FIFO_occupation)
Artiom 0:bc74da1c502f 67 FIFO_max_occupation=FIFO_occupation;
maximilienlv 47:ced7758fb2ce 68
Artiom 0:bc74da1c502f 69 if(FIFO_occupation!=0) {
Artiom 0:bc74da1c502f 70 int identifiant=msgRxBuffer[FIFO_lecture].id;
Artiom 0:bc74da1c502f 71
maximilienlv 47:ced7758fb2ce 72 switch(identifiant)
maximilienlv 47:ced7758fb2ce 73 {
marwanesaich 45:11614fc23e53 74
marwanesaich 45:11614fc23e53 75 case GLOBAL_START:
marwanesaich 45:11614fc23e53 76 EtatGameStart = 1;
maximilienlv 47:ced7758fb2ce 77 EtatGameEnd = 0;
marwanesaich 45:11614fc23e53 78 break;
marwanesaich 45:11614fc23e53 79 case RECALAGE_START :
marwanesaich 45:11614fc23e53 80 EtatGameRecalage = 1;
marwanesaich 45:11614fc23e53 81 break;
marwanesaich 45:11614fc23e53 82
marwanesaich 45:11614fc23e53 83 case INSTRUCTION_END_MOTEUR:
marwanesaich 45:11614fc23e53 84 ackFinAction = msgRxBuffer[FIFO_lecture].data[0]|((unsigned short)(msgRxBuffer[FIFO_lecture].data[1])<<8);
marwanesaich 45:11614fc23e53 85 break;
Artiom 32:e9947815c4d7 86
Artiom 0:bc74da1c502f 87 case GLOBAL_GAME_END:
Artiom 0:bc74da1c502f 88 EtatGameEnd = 1;
maximilienlv 47:ced7758fb2ce 89 EtatGameStart = 0;
Artiom 0:bc74da1c502f 90 break;
Artiom 0:bc74da1c502f 91
Artiom 0:bc74da1c502f 92 case DATA_TELEMETRE: //Lit le telemetre N°X suivant la data dans le CAN
Artiom 0:bc74da1c502f 93 char numero_telemetre=msgRxBuffer[FIFO_lecture].data[0];
Artiom 0:bc74da1c502f 94 short distance=lecture_telemetre(numero_telemetre);
Artiom 0:bc74da1c502f 95
Artiom 0:bc74da1c502f 96
Artiom 0:bc74da1c502f 97 msgTx.id=RECEPTION_DATA; // tx Valeur Telemetre1
Artiom 0:bc74da1c502f 98 msgTx.len=2;
Artiom 0:bc74da1c502f 99 msgTx.format=CANStandard;
Artiom 0:bc74da1c502f 100 msgTx.type=CANData;
Artiom 0:bc74da1c502f 101 // Rayon sur 2 octets
Artiom 0:bc74da1c502f 102 msgTx.data[0]=(unsigned char)distance;
Artiom 0:bc74da1c502f 103 msgTx.data[1]=(unsigned char)(distance>>8);
Artiom 0:bc74da1c502f 104 can.write(msgTx);
Artiom 0:bc74da1c502f 105 SendAck(ACKNOWLEDGE_TELEMETRE,RECEPTION_DATA);
Artiom 0:bc74da1c502f 106 break;
Artiom 0:bc74da1c502f 107
Artiom 0:bc74da1c502f 108 case DATA_RECALAGE:
Artiom 0:bc74da1c502f 109 short distance1=lecture_telemetre(1);
Artiom 0:bc74da1c502f 110 short distance2=lecture_telemetre(2);
Artiom 0:bc74da1c502f 111 short distance3=lecture_telemetre(3);
Artiom 0:bc74da1c502f 112 short distance4=lecture_telemetre(4);
Artiom 0:bc74da1c502f 113
Artiom 0:bc74da1c502f 114 msgTx.id=RECEPTION_RECALAGE; // tx Valeur Telemetre1
Artiom 0:bc74da1c502f 115 msgTx.len=8;
Artiom 0:bc74da1c502f 116 msgTx.format=CANStandard;
Artiom 0:bc74da1c502f 117 msgTx.type=CANData;
Artiom 0:bc74da1c502f 118 // Rayon sur 2 octets
Artiom 0:bc74da1c502f 119 msgTx.data[0]=(unsigned char)distance1;
Artiom 0:bc74da1c502f 120 msgTx.data[1]=(unsigned char)(distance1>>8);
Artiom 0:bc74da1c502f 121 msgTx.data[2]=(unsigned char)distance2;
Artiom 0:bc74da1c502f 122 msgTx.data[3]=(unsigned char)(distance2>>8);
Artiom 0:bc74da1c502f 123 msgTx.data[4]=(unsigned char)distance3;
Artiom 0:bc74da1c502f 124 msgTx.data[5]=(unsigned char)(distance3>>8);
Artiom 0:bc74da1c502f 125 msgTx.data[6]=(unsigned char)distance4;
Artiom 0:bc74da1c502f 126 msgTx.data[7]=(unsigned char)(distance4>>8);
Artiom 0:bc74da1c502f 127 can.write(msgTx);
Artiom 0:bc74da1c502f 128 SendAck(ACKNOWLEDGE_TELEMETRE,RECEPTION_RECALAGE);
Artiom 0:bc74da1c502f 129 break;
Artiom 44:381ecf63e6ab 130
Artiom 44:381ecf63e6ab 131 case DATA_TELEMETRE_LOGIQUE:
Artiom 43:7fff7f4d79a1 132 msgTx.id=RECEPTION_TELEMETRE_LOGIQUE; // tx Valeur Telemetre1
Artiom 43:7fff7f4d79a1 133 msgTx.len=4;
Artiom 43:7fff7f4d79a1 134 msgTx.format=CANStandard;
Artiom 43:7fff7f4d79a1 135 msgTx.type=CANData;
Artiom 43:7fff7f4d79a1 136 msgTx.data[0]=(unsigned char)DT1_interrupt_Ex;
Artiom 43:7fff7f4d79a1 137 msgTx.data[1]=(unsigned char)DT2_interrupt_Ex;
Artiom 43:7fff7f4d79a1 138 msgTx.data[2]=(unsigned char)DT3_interrupt_Ex;
Artiom 43:7fff7f4d79a1 139 msgTx.data[3]=(unsigned char)DT4_interrupt_Ex;
Artiom 43:7fff7f4d79a1 140 can.write(msgTx);
Artiom 43:7fff7f4d79a1 141 SendAck(ACKNOWLEDGE_TELEMETRE,RECEPTION_TELEMETRE_LOGIQUE);
Artiom 43:7fff7f4d79a1 142 break;
maximilienlv 47:ced7758fb2ce 143
maximilienlv 47:ced7758fb2ce 144 //----------------------------------------------------------------nouveaux case-----------------------------------------------------------------//
maximilienlv 47:ced7758fb2ce 145
maximilienlv 47:ced7758fb2ce 146 case TEST_BRAS_A:
kyxstark 38:9295a9721acb 147 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
marwanesaich 45:11614fc23e53 148 verification();
marwanesaich 45:11614fc23e53 149 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
marwanesaich 45:11614fc23e53 150 break;
maximilienlv 47:ced7758fb2ce 151
maximilienlv 47:ced7758fb2ce 152 case TEST_BRAS_B:
maximilienlv 47:ced7758fb2ce 153 gabarit_robot();
maximilienlv 47:ced7758fb2ce 154 break;
maximilienlv 47:ced7758fb2ce 155
maximilienlv 47:ced7758fb2ce 156 case TEST_BRAS_C:
maximilienlv 47:ced7758fb2ce 157 //ecueil_arriere_at();
Artiom 0:bc74da1c502f 158 break;
maximilienlv 47:ced7758fb2ce 159
maximilienlv 47:ced7758fb2ce 160 case TEST_BRAS_D:
maximilienlv 47:ced7758fb2ce 161 //test_BRAS_D();
maximilienlv 47:ced7758fb2ce 162 break;
maximilienlv 47:ced7758fb2ce 163
maximilienlv 47:ced7758fb2ce 164 case TEST_BRAS_1:
maximilienlv 47:ced7758fb2ce 165 //test_BRAS_1();
Artiom 31:29500874c1cc 166 break;
maximilienlv 47:ced7758fb2ce 167
maximilienlv 47:ced7758fb2ce 168 case TEST_BRAS_2:
maximilienlv 47:ced7758fb2ce 169 //test_BRAS_2();
maximilienlv 47:ced7758fb2ce 170 break;
maximilienlv 47:ced7758fb2ce 171
maximilienlv 47:ced7758fb2ce 172 case TEST_BRAS_3:
maximilienlv 47:ced7758fb2ce 173 //test_BRAS_3();
Artiom 2:9e63099cca99 174 break;
maximilienlv 47:ced7758fb2ce 175
maximilienlv 47:ced7758fb2ce 176 case TEST_BRAS_4:
maximilienlv 47:ced7758fb2ce 177 //test_BRAS_4();
Artiom 31:29500874c1cc 178 break;
maximilienlv 47:ced7758fb2ce 179
maximilienlv 47:ced7758fb2ce 180 case TEST_BRAS_5:
maximilienlv 47:ced7758fb2ce 181 //test_BRAS_5();
Artiom 2:9e63099cca99 182 break;
maximilienlv 47:ced7758fb2ce 183
maximilienlv 47:ced7758fb2ce 184 case TEST_BRAS_6:
maximilienlv 47:ced7758fb2ce 185 //test_BRAS_6();
Artiom 2:9e63099cca99 186 break;
maximilienlv 47:ced7758fb2ce 187 ////////////////////////////////////////////CASE DE COMPETITIONS(à optimiser)////////////////////////////////////////
maximilienlv 47:ced7758fb2ce 188 case BRAS_AT:
maximilienlv 47:ced7758fb2ce 189 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
maximilienlv 47:ced7758fb2ce 190 bras_choix = msgRxBuffer[FIFO_lecture].data[0];
maximilienlv 47:ced7758fb2ce 191 //var_bras_at = 1;
maximilienlv 47:ced7758fb2ce 192 pc.printf("%d\n",bras_choix);
maximilienlv 47:ced7758fb2ce 193 competition_bras_at(bras_choix);
maximilienlv 47:ced7758fb2ce 194 verification();
maximilienlv 47:ced7758fb2ce 195 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom 0:bc74da1c502f 196 break;
maximilienlv 47:ced7758fb2ce 197
maximilienlv 47:ced7758fb2ce 198 case BRAS_RE:
Artiom 14:b1abd2f6df75 199 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
maximilienlv 47:ced7758fb2ce 200 bras_choix = msgRxBuffer[FIFO_lecture].data[0];
maximilienlv 47:ced7758fb2ce 201 //var_bras_re = 1;
maximilienlv 47:ced7758fb2ce 202 pc.printf("%d\n",bras_choix);
maximilienlv 47:ced7758fb2ce 203 competition_bras_re(bras_choix);
Artiom 14:b1abd2f6df75 204 verification();
maximilienlv 47:ced7758fb2ce 205 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom 14:b1abd2f6df75 206 break;
maximilienlv 47:ced7758fb2ce 207
maximilienlv 47:ced7758fb2ce 208 case GABARIT_ROBOT:
maximilienlv 47:ced7758fb2ce 209 gabarit_robot();
maximilienlv 47:ced7758fb2ce 210 verification();
maximilienlv 47:ced7758fb2ce 211 break;
maximilienlv 47:ced7758fb2ce 212
maximilienlv 47:ced7758fb2ce 213 case ECUEIL_ARRIERE_AT:
Artiom 31:29500874c1cc 214 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
maximilienlv 47:ced7758fb2ce 215 //var_ecueil_arriere_at = 1;
Artiom 31:29500874c1cc 216 verification();
Artiom 31:29500874c1cc 217 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom 31:29500874c1cc 218 break;
maximilienlv 47:ced7758fb2ce 219
maximilienlv 47:ced7758fb2ce 220 case ECUEIL_AVANT_AT:
Artiom 20:42f8ec726ac8 221 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
maximilienlv 47:ced7758fb2ce 222 //var_ecueil_avant_at = 1;
Artiom 20:42f8ec726ac8 223 verification();
Artiom 20:42f8ec726ac8 224 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom 20:42f8ec726ac8 225 break;
maximilienlv 47:ced7758fb2ce 226
maximilienlv 47:ced7758fb2ce 227 default:
marwanesaich 45:11614fc23e53 228 break;
Artiom 0:bc74da1c502f 229 }
Artiom 0:bc74da1c502f 230 FIFO_lecture=(FIFO_lecture+1)%SIZE_FIFO;
Artiom 0:bc74da1c502f 231 }
maximilienlv 47:ced7758fb2ce 232 }