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@52:c019890fbc59, 2021-05-18 (annotated)

Committer:
goldmas
Date:
Tue May 18 10:05:06 2021 +0000
Revision:
52:c019890fbc59
Parent:
51:ecbdff4587ce
Child:
53:b7d2682fba44
code avec manche a air et new identifiants fct

Who changed what in which revision?

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