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@49:af201920161a, 2020-07-09 (annotated)

Committer:
maximilienlv
Date:
Thu Jul 09 18:31:15 2020 +0000
Revision:
49:af201920161a
Parent:
48:ef0dc0df8c77
Child:
50:85b7a7c57d21
ligne de code pour tester les capteurs de couleurs;

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
kyxstark 21:edf6d03fdb20 18 void canProcessRx(void);
Artiom 12:191486ba5118 19
Artiom 0:bc74da1c502f 20 /*********************************************************************************************/
Artiom 0:bc74da1c502f 21 /* FUNCTION NAME: canRx_ISR */
Artiom 0:bc74da1c502f 22 /* DESCRIPTION : lit les messages sur le can et les stocke dans la FIFO */
Artiom 0:bc74da1c502f 23 /*********************************************************************************************/
Artiom 0:bc74da1c502f 24 void canRx_ISR (void)
Artiom 0:bc74da1c502f 25 {
maximilienlv 47:ced7758fb2ce 26 if (can.read(msgRxBuffer[FIFO_ecriture]))
maximilienlv 47:ced7758fb2ce 27 {
maximilienlv 47:ced7758fb2ce 28 FIFO_ecriture=(FIFO_ecriture+1)%SIZE_FIFO;
Artiom 0:bc74da1c502f 29 }
Artiom 0:bc74da1c502f 30 }
Artiom 0:bc74da1c502f 31
Artiom 0:bc74da1c502f 32 int main()
Artiom 0:bc74da1c502f 33 {
Artiom 0:bc74da1c502f 34 can.attach(&canRx_ISR); // création de l'interrupt attachée à la réception sur le CAN
Artiom 0:bc74da1c502f 35 servo_interrupt_en(); //permettre les interuptions
maximilienlv 47:ced7758fb2ce 36 wait_ms(200);
Artiom 32:e9947815c4d7 37 deverouillage_torque();
maximilienlv 47:ced7758fb2ce 38 pc.printf("\nLAUNCHED\n\r");
maximilienlv 47:ced7758fb2ce 39 //on rentre tous les bras dans le robot dans le vagin de sa mere
maximilienlv 48:ef0dc0df8c77 40 gabarit_robot_droit();
maximilienlv 48:ef0dc0df8c77 41 wait(1);
maximilienlv 48:ef0dc0df8c77 42 gabarit_robot_gauche();
maximilienlv 48:ef0dc0df8c77 43 wait(1);
maximilienlv 49:af201920161a 44 //DigitalIn cap2(PC_14);
maximilienlv 47:ced7758fb2ce 45 while(1)
maximilienlv 49:af201920161a 46 {
maximilienlv 49:af201920161a 47 Get_Led(0,1);
maximilienlv 49:af201920161a 48 wait(1);
maximilienlv 49:af201920161a 49
maximilienlv 49:af201920161a 50 /*uint8_t test_color = Get_Led(0,1);
maximilienlv 49:af201920161a 51 SendMsgCan(0xAA,&test_color,2);*/
maximilienlv 49:af201920161a 52 /* if(cap2.read())
maximilienlv 49:af201920161a 53 {
maximilienlv 49:af201920161a 54 }else{
maximilienlv 49:af201920161a 55 }*/
Artiom 0:bc74da1c502f 56 canProcessRx();
Artiom 10:48c0d1b8aed9 57 f_mesure();//dt35
maximilienlv 48:ef0dc0df8c77 58
maximilienlv 49:af201920161a 59 automate_bras_attraper_1();
maximilienlv 49:af201920161a 60 automate_bras_relacher_1();
maximilienlv 49:af201920161a 61 automate_bras_attraper_2();
maximilienlv 49:af201920161a 62 automate_bras_relacher_2();
maximilienlv 49:af201920161a 63 automate_bras_attraper_3();
maximilienlv 49:af201920161a 64 automate_bras_relacher_3();
maximilienlv 49:af201920161a 65 }
Artiom 0:bc74da1c502f 66 }
maximilienlv 47:ced7758fb2ce 67 //fin du main
Artiom 0:bc74da1c502f 68
Artiom 0:bc74da1c502f 69 /****************************************************************************************/
Artiom 0:bc74da1c502f 70 /* FUNCTION NAME: canProcessRx */
Artiom 0:bc74da1c502f 71 /* DESCRIPTION : Fonction de traitement des messages CAN */
Artiom 0:bc74da1c502f 72 /****************************************************************************************/
Artiom 0:bc74da1c502f 73 void canProcessRx(void)
Artiom 0:bc74da1c502f 74 {
Artiom 0:bc74da1c502f 75 static signed char FIFO_occupation=0,FIFO_max_occupation=0;
Artiom 0:bc74da1c502f 76 CANMessage msgTx=CANMessage();
maximilienlv 49:af201920161a 77
Artiom 0:bc74da1c502f 78 FIFO_occupation=FIFO_ecriture-FIFO_lecture;
Artiom 0:bc74da1c502f 79 if(FIFO_occupation<0)
Artiom 0:bc74da1c502f 80 FIFO_occupation=FIFO_occupation+SIZE_FIFO;
maximilienlv 49:af201920161a 81
Artiom 0:bc74da1c502f 82 if(FIFO_max_occupation<FIFO_occupation)
Artiom 0:bc74da1c502f 83 FIFO_max_occupation=FIFO_occupation;
maximilienlv 49:af201920161a 84
Artiom 0:bc74da1c502f 85 if(FIFO_occupation!=0) {
Artiom 0:bc74da1c502f 86 int identifiant=msgRxBuffer[FIFO_lecture].id;
Artiom 0:bc74da1c502f 87
maximilienlv 47:ced7758fb2ce 88 switch(identifiant)
maximilienlv 47:ced7758fb2ce 89 {
marwanesaich 45:11614fc23e53 90 case GLOBAL_START:
marwanesaich 45:11614fc23e53 91 EtatGameStart = 1;
maximilienlv 47:ced7758fb2ce 92 EtatGameEnd = 0;
marwanesaich 45:11614fc23e53 93 break;
marwanesaich 45:11614fc23e53 94 case RECALAGE_START :
marwanesaich 45:11614fc23e53 95 EtatGameRecalage = 1;
marwanesaich 45:11614fc23e53 96 break;
marwanesaich 45:11614fc23e53 97
marwanesaich 45:11614fc23e53 98 case INSTRUCTION_END_MOTEUR:
marwanesaich 45:11614fc23e53 99 ackFinAction = msgRxBuffer[FIFO_lecture].data[0]|((unsigned short)(msgRxBuffer[FIFO_lecture].data[1])<<8);
marwanesaich 45:11614fc23e53 100 break;
Artiom 32:e9947815c4d7 101
Artiom 0:bc74da1c502f 102 case GLOBAL_GAME_END:
Artiom 0:bc74da1c502f 103 EtatGameEnd = 1;
maximilienlv 47:ced7758fb2ce 104 EtatGameStart = 0;
Artiom 0:bc74da1c502f 105 break;
Artiom 0:bc74da1c502f 106
Artiom 0:bc74da1c502f 107 case DATA_TELEMETRE: //Lit le telemetre N°X suivant la data dans le CAN
Artiom 0:bc74da1c502f 108 char numero_telemetre=msgRxBuffer[FIFO_lecture].data[0];
Artiom 0:bc74da1c502f 109 short distance=lecture_telemetre(numero_telemetre);
Artiom 0:bc74da1c502f 110
Artiom 0:bc74da1c502f 111
Artiom 0:bc74da1c502f 112 msgTx.id=RECEPTION_DATA; // tx Valeur Telemetre1
Artiom 0:bc74da1c502f 113 msgTx.len=2;
Artiom 0:bc74da1c502f 114 msgTx.format=CANStandard;
Artiom 0:bc74da1c502f 115 msgTx.type=CANData;
Artiom 0:bc74da1c502f 116 // Rayon sur 2 octets
Artiom 0:bc74da1c502f 117 msgTx.data[0]=(unsigned char)distance;
Artiom 0:bc74da1c502f 118 msgTx.data[1]=(unsigned char)(distance>>8);
Artiom 0:bc74da1c502f 119 can.write(msgTx);
Artiom 0:bc74da1c502f 120 SendAck(ACKNOWLEDGE_TELEMETRE,RECEPTION_DATA);
Artiom 0:bc74da1c502f 121 break;
Artiom 0:bc74da1c502f 122
Artiom 0:bc74da1c502f 123 case DATA_RECALAGE:
Artiom 0:bc74da1c502f 124 short distance1=lecture_telemetre(1);
Artiom 0:bc74da1c502f 125 short distance2=lecture_telemetre(2);
Artiom 0:bc74da1c502f 126 short distance3=lecture_telemetre(3);
Artiom 0:bc74da1c502f 127 short distance4=lecture_telemetre(4);
Artiom 0:bc74da1c502f 128
Artiom 0:bc74da1c502f 129 msgTx.id=RECEPTION_RECALAGE; // tx Valeur Telemetre1
Artiom 0:bc74da1c502f 130 msgTx.len=8;
Artiom 0:bc74da1c502f 131 msgTx.format=CANStandard;
Artiom 0:bc74da1c502f 132 msgTx.type=CANData;
Artiom 0:bc74da1c502f 133 // Rayon sur 2 octets
Artiom 0:bc74da1c502f 134 msgTx.data[0]=(unsigned char)distance1;
Artiom 0:bc74da1c502f 135 msgTx.data[1]=(unsigned char)(distance1>>8);
Artiom 0:bc74da1c502f 136 msgTx.data[2]=(unsigned char)distance2;
Artiom 0:bc74da1c502f 137 msgTx.data[3]=(unsigned char)(distance2>>8);
Artiom 0:bc74da1c502f 138 msgTx.data[4]=(unsigned char)distance3;
Artiom 0:bc74da1c502f 139 msgTx.data[5]=(unsigned char)(distance3>>8);
Artiom 0:bc74da1c502f 140 msgTx.data[6]=(unsigned char)distance4;
Artiom 0:bc74da1c502f 141 msgTx.data[7]=(unsigned char)(distance4>>8);
Artiom 0:bc74da1c502f 142 can.write(msgTx);
Artiom 0:bc74da1c502f 143 SendAck(ACKNOWLEDGE_TELEMETRE,RECEPTION_RECALAGE);
Artiom 0:bc74da1c502f 144 break;
Artiom 44:381ecf63e6ab 145
Artiom 44:381ecf63e6ab 146 case DATA_TELEMETRE_LOGIQUE:
Artiom 43:7fff7f4d79a1 147 msgTx.id=RECEPTION_TELEMETRE_LOGIQUE; // tx Valeur Telemetre1
Artiom 43:7fff7f4d79a1 148 msgTx.len=4;
Artiom 43:7fff7f4d79a1 149 msgTx.format=CANStandard;
Artiom 43:7fff7f4d79a1 150 msgTx.type=CANData;
Artiom 43:7fff7f4d79a1 151 msgTx.data[0]=(unsigned char)DT1_interrupt_Ex;
Artiom 43:7fff7f4d79a1 152 msgTx.data[1]=(unsigned char)DT2_interrupt_Ex;
Artiom 43:7fff7f4d79a1 153 msgTx.data[2]=(unsigned char)DT3_interrupt_Ex;
Artiom 43:7fff7f4d79a1 154 msgTx.data[3]=(unsigned char)DT4_interrupt_Ex;
Artiom 43:7fff7f4d79a1 155 can.write(msgTx);
Artiom 43:7fff7f4d79a1 156 SendAck(ACKNOWLEDGE_TELEMETRE,RECEPTION_TELEMETRE_LOGIQUE);
Artiom 43:7fff7f4d79a1 157 break;
maximilienlv 49:af201920161a 158
maximilienlv 49:af201920161a 159 //----------------------------------------------------------------cases test-----------------------------------------------------------------//
maximilienlv 47:ced7758fb2ce 160
maximilienlv 47:ced7758fb2ce 161 case TEST_BRAS_A:
maximilienlv 49:af201920161a 162 aut_bras_av_3_at = 1;
maximilienlv 49:af201920161a 163 bras_choix = 0;
marwanesaich 45:11614fc23e53 164 break;
maximilienlv 47:ced7758fb2ce 165
maximilienlv 47:ced7758fb2ce 166 case TEST_BRAS_B:
maximilienlv 49:af201920161a 167 aut_bras_av_3_re = 1;
maximilienlv 49:af201920161a 168 bras_choix = 0;
maximilienlv 47:ced7758fb2ce 169 break;
maximilienlv 47:ced7758fb2ce 170
maximilienlv 47:ced7758fb2ce 171 case TEST_BRAS_C:
maximilienlv 49:af201920161a 172 aut_bras_av_3_at = 1;
maximilienlv 49:af201920161a 173 bras_choix = 1;
Artiom 0:bc74da1c502f 174 break;
maximilienlv 47:ced7758fb2ce 175
maximilienlv 47:ced7758fb2ce 176 case TEST_BRAS_D:
maximilienlv 49:af201920161a 177 aut_bras_av_3_re = 1;
maximilienlv 49:af201920161a 178 bras_choix = 1;
maximilienlv 47:ced7758fb2ce 179 break;
maximilienlv 49:af201920161a 180
maximilienlv 49:af201920161a 181 case TEST_BRAS_1:
maximilienlv 49:af201920161a 182 test_BRAS_1();
maximilienlv 47:ced7758fb2ce 183 break;
maximilienlv 47:ced7758fb2ce 184
maximilienlv 49:af201920161a 185 case TEST_BRAS_2:
maximilienlv 49:af201920161a 186 test_BRAS_2();
Artiom 2:9e63099cca99 187 break;
maximilienlv 47:ced7758fb2ce 188
maximilienlv 49:af201920161a 189 case TEST_BRAS_3:
maximilienlv 49:af201920161a 190 test_BRAS_3();
maximilienlv 49:af201920161a 191 break;
maximilienlv 49:af201920161a 192
maximilienlv 49:af201920161a 193 case TEST_BRAS_4:
maximilienlv 49:af201920161a 194 test_BRAS_4();
Artiom 31:29500874c1cc 195 break;
maximilienlv 47:ced7758fb2ce 196
maximilienlv 48:ef0dc0df8c77 197 case TEST_BRAS_5:
Artiom 2:9e63099cca99 198 break;
maximilienlv 47:ced7758fb2ce 199
maximilienlv 48:ef0dc0df8c77 200 case TEST_BRAS_6:
Artiom 2:9e63099cca99 201 break;
maximilienlv 49:af201920161a 202 ////////////////////////////////////////////CASE DE COMPETITIONS/////////////////////////////////////////////
maximilienlv 49:af201920161a 203 case BRAS_AT_1:
maximilienlv 47:ced7758fb2ce 204 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
maximilienlv 47:ced7758fb2ce 205 bras_choix = msgRxBuffer[FIFO_lecture].data[0];
maximilienlv 49:af201920161a 206 aut_bras_av_1_at = 1;
maximilienlv 49:af201920161a 207 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
maximilienlv 49:af201920161a 208 break;
maximilienlv 49:af201920161a 209
maximilienlv 49:af201920161a 210 case BRAS_RE_1:
maximilienlv 49:af201920161a 211 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
maximilienlv 49:af201920161a 212 bras_choix = msgRxBuffer[FIFO_lecture].data[0];
maximilienlv 49:af201920161a 213 aut_bras_av_1_re = 1;
maximilienlv 49:af201920161a 214 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
maximilienlv 49:af201920161a 215 break;
maximilienlv 49:af201920161a 216
maximilienlv 49:af201920161a 217 case BRAS_AT_2:
maximilienlv 49:af201920161a 218 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
maximilienlv 49:af201920161a 219 bras_choix = msgRxBuffer[FIFO_lecture].data[0];
maximilienlv 49:af201920161a 220 aut_bras_av_2_at = 1;
maximilienlv 47:ced7758fb2ce 221 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom 0:bc74da1c502f 222 break;
maximilienlv 47:ced7758fb2ce 223
maximilienlv 49:af201920161a 224 case BRAS_RE_2:
maximilienlv 49:af201920161a 225 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
maximilienlv 49:af201920161a 226 bras_choix = msgRxBuffer[FIFO_lecture].data[0];
maximilienlv 49:af201920161a 227 aut_bras_av_2_re = 1;
maximilienlv 49:af201920161a 228 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
maximilienlv 49:af201920161a 229 break;
maximilienlv 49:af201920161a 230
maximilienlv 49:af201920161a 231 case BRAS_AT_3:
Artiom 14:b1abd2f6df75 232 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
maximilienlv 47:ced7758fb2ce 233 bras_choix = msgRxBuffer[FIFO_lecture].data[0];
maximilienlv 49:af201920161a 234 aut_bras_av_3_at = 1;
maximilienlv 49:af201920161a 235 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
maximilienlv 49:af201920161a 236 break;
maximilienlv 49:af201920161a 237
maximilienlv 49:af201920161a 238 case BRAS_RE_3:
maximilienlv 49:af201920161a 239 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
maximilienlv 49:af201920161a 240 bras_choix = msgRxBuffer[FIFO_lecture].data[0];
maximilienlv 49:af201920161a 241 aut_bras_av_3_re = 1;
maximilienlv 49:af201920161a 242 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom 14:b1abd2f6df75 243 break;
maximilienlv 47:ced7758fb2ce 244
maximilienlv 47:ced7758fb2ce 245 case GABARIT_ROBOT:
maximilienlv 49:af201920161a 246 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
maximilienlv 48:ef0dc0df8c77 247 gabarit_robot_droit();
maximilienlv 48:ef0dc0df8c77 248 gabarit_robot_gauche();
Artiom 20:42f8ec726ac8 249 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom 20:42f8ec726ac8 250 break;
maximilienlv 47:ced7758fb2ce 251
maximilienlv 47:ced7758fb2ce 252 default:
marwanesaich 45:11614fc23e53 253 break;
Artiom 0:bc74da1c502f 254 }
Artiom 0:bc74da1c502f 255 FIFO_lecture=(FIFO_lecture+1)%SIZE_FIFO;
Artiom 0:bc74da1c502f 256 }
maximilienlv 47:ced7758fb2ce 257 }