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@32:e9947815c4d7, 2019-05-23 (annotated)

Committer:
Artiom
Date:
Thu May 23 10:42:28 2019 +0000
Revision:
32:e9947815c4d7
Parent:
31:29500874c1cc
Child:
35:5df59d61c95d
Child:
36:04c3ad41064d
amelioration automate balance;

Who changed what in which revision?

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