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@50:85b7a7c57d21, 2020-07-17 (annotated)

Committer:
maximilienlv
Date:
Fri Jul 17 14:33:58 2020 +0000
Revision:
50:85b7a7c57d21
Parent:
49:af201920161a
Child:
51:ecbdff4587ce
modif ACK; fonctionnel en start

Who changed what in which revision?

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