Jimmy Maingam
sequences des strats
Diff: Strategie.cpp
- Revision:
- 9:00361ecc429d
- Parent:
- 8:55fe932fd696
- Child:
- 10:c27f3e788466
--- a/Strategie.cpp Thu May 30 08:31:58 2019 +0000
+++ b/Strategie.cpp Fri May 31 19:05:28 2019 +0000
@@ -19,9 +19,11 @@
extern bool stopCapteurs;
+extern Serial pc;
+
Timeout tempsArretMvt;
-
+bool actionLancer = false;
void updateAndWriteScore(char n1, char n2, char n3)
@@ -32,20 +34,21 @@
write(bufferScore);
}
+
void LectureI2CCarteCapteur(deplacement robot)
{
+ pc.printf("Avant le traitement\n\r");
traitementBufferCapteurs();
- //if(AnalyseDistance(distanceUltrasonGauche, distanceUltrasonDroit, distanceUltrasonArriere, typeMovement) == ROBOT) {
- if( ((distanceUltrasonGauche <= 300) || (distanceUltrasonDroit <= 300)) && (typeMovement == DEPLACEMENT_AVANT) ) {
- stopCapteurs = true;
- } else if ( (distanceUltrasonArriere <= 300) && (typeMovement == DEPLACEMENT_ARRIERE) ) {
+ pc.printf("Apres le traitement\n\r");
+ if(AnalyseDistance(distanceUltrasonGauche, distanceUltrasonDroit, distanceUltrasonArriere, typeMovement) == ROBOT) {
+ if( ((distanceUltrasonGauche <= 300) || (distanceUltrasonDroit <= 300)) || (distanceUltrasonArriere <= 300) && (typeMovement == DEPLACEMENT_AVANT) ) {
stopCapteurs = true;
} else {
stopCapteurs = false;
}
- /* } else {
- stopCapteurs = false;
- }*/
+ } else {
+ stopCapteurs = false;
+ }
}
void arretSystem()
@@ -54,7 +57,12 @@
deplacement robot;
robot.initialisation();
- robot.arreterRobot();
+ set_PWM_moteur_D(0);
+ set_PWM_moteur_G(0);
+ motors_stop();
+ stopPompe();
+ while(1);
+ //robot.arreterRobot();
}
/*---------Debut Strategie homologation violet ------------------------------------------------------*/
@@ -84,18 +92,19 @@
switch (etapeMatch) {
case 0 : { /* On tourne pour aller vers le terrain adverse et on avance jusqu'au milieu du terrain */
-
+
typeMovement = DEPLACEMENT_AVANT;
- robot.rotation_rel(-90);
- robot.ligne_droite_basique(50000);
+ setEmplacementTest();
+ init_odometrie();
robot.rotation_rel(90);
robot.ligne_droite_basique(50000);
- robot.rotation_rel(90);
+ robot.rotation_rel(-90);
+ robot.ligne_droite_basique(50000);
+ robot.rotation_rel(-90);
robot.ligne_droite_basique(50000);
- robot.rotation_rel(90);
+ robot.rotation_rel(-90);
robot.ligne_droite_basique(50000);
- robot.rotation_rel(90);
- //robot.ligne_droite_basique(127500);
+ robot.rotation_rel(-90);
etapeMatch++;
break;
@@ -131,8 +140,8 @@
//pc.printf("On attend le depart du Robot Thanos\n\r");
}
tempsArretMvt.attach(&arretSystem, TEMPSMATCH);
- initJaune();
- robot.initialisation();
+ //initJaune();
+ //robot.initialisation();
lancerTimerEcran();
tempsMatch.start();
typeEvitement = ARRET;
@@ -140,9 +149,14 @@
switch (etapeMatch) {
case 0 : { /* On tourne pour aller vers le terrain adverse et on avance jusqu'au milieu du terrain */
+
typeMovement = DEPLACEMENT_AVANT;
- robot.ligne_droite_basique(3000);
- robot.rotation_rel(-90);
+ setEmplacementTest();
+ init_odometrie();
+ robot.va_au_point(80000,30000,-90);
+ robot.va_au_point(80000,80000,-90);
+ robot.va_au_point(30000,80000,-90);
+ robot.va_au_point(30000,30000,-90);
//robot.ligne_droite_basique(127500);
etapeMatch++;
@@ -152,6 +166,7 @@
robot.vitesse_nulle_D(0);
robot.vitesse_nulle_G(0);
motors_stop();
+ stopPompe();
}
}
}
@@ -160,6 +175,7 @@
robot.vitesse_nulle_D(0);
robot.vitesse_nulle_G(0);
motors_stop();
+ stopPompe();
while(1);
}
@@ -174,32 +190,333 @@
write(bufferScore);
Timer tempsMatch; // timer en seconde
int etapeMatch = 0;
+ initViolet();
while(demarreur.getState() == false) {
//on attend
- //pc.printf("On attend le depart du Robot Thanos\n\r");
+ pc.printf("On attend le depart du Robot Thanos\n\r");
}
tempsArretMvt.attach(&arretSystem, TEMPSMATCH);
- initViolet();
- robot.initialisation();//check si modif
+ //robot.initialisation();//check si modif
lancerTimerEcran();
tempsMatch.start();
- typeEvitement = ARRET;
+ //typeEvitement = ARRET;
+
while(tempsMatch <= TEMPSMATCH) {
switch (etapeMatch) {
- case 0 : { /* On active l'expérience */
+ case 0 : { /* on prend les 3 palets du petit distributeur */
+ typeMovement = DEPLACEMENT_AVANT;
+ typeEvitement = ARRET;
+ robot.va_au_point(27500,178000,90);
+ etapeMatch++;
+ break;
+ }
+ case 1: {
+ if(actionLancer != true) {
+ prendre3Palets();
+ actionLancer = true;
+ }
+
+ if(finActions == true) {
+ finActions = false;
+ actionLancer = false;
+ etapeMatch++;
+ }
+ break;
+ }
+ case 2 : { /* On se place entre la case rouge et verte pour poser le palet rouge */
+ robot.poussette(2000);
+ typeMovement = DEPLACEMENT_ARRIERE;
+ robot.marche_arriere(-50000);
+ robot.rotation_abs(-90);
+ typeMovement = DEPLACEMENT_AVANT;
+ robot.va_au_point(22500,60000,-90);
+ etapeMatch++;
+ break;
+ }
+ case 3 : {
+ if(actionLancer != true) {
+ lacherVentouse3();
+ actionLancer = true;
+ }
+ if(finActions == true) {
+ updateAndWriteScore('0', '0', '6');
+ finActions = false;
+ actionLancer = false;
+ etapeMatch++;
+ }
+ break;
+ }
+ case 4 : { /* On se place entre la case verte et bleue pour poser le palet vert */
+ typeMovement = DEPLACEMENT_ARRIERE;
+ wait(2);
+ robot.marche_arriere(-30000);
+ etapeMatch++;
+ break;
+ }
+ case 5 : {
+ if(actionLancer != true) {
+ lacherVentouse2();
+ actionLancer = true;
+ }
+ if(finActions == true) {
+ updateAndWriteScore('0', '1', '2');
+ finActions = false;
+ actionLancer = false;
+ etapeMatch++;
+ }
+ break;
+ }
+ case 6 : { /* On se place en bordure de la case bleue pour poser le palet bleu */
+ wait(2);
+ robot.marche_arriere(-30000);
+
+ etapeMatch++;
+ break;
+ }
+ case 7 : {
+ if(actionLancer != true) {
+ lacherVentouse1();
+ stopPompe();
+ actionLancer = true;
+ }
+ if(finActions == true) {
+ updateAndWriteScore('0', '1', '8');
+ finActions = false;
+ actionLancer = false;
+ etapeMatch++;
+ }
+ break;
+ }
+ case 8 : {
+ wait(2);
+ robot.marche_arriere(-50000);
+ etapeMatch = 30;;
+ break;
+ }
+ case 9 : { /* on est placé devant le grand distributeur pour prendre les 4 premiers palets */
+ typeMovement = DEPLACEMENT_AVANT;
+ //robot.va_au_point(70000,130000,90);
+ robot.va_au_point(65000,130000,90);
+ robot.ligne_droite_basique(5000);
+ etapeMatch++;
+ break;
+ }
+ case 10 : {
+ if(actionLancer != true) {
+ prendre4Palets();
+ actionLancer = true;
+ }
+ if(finActions == true) {
+ finActions = false;
+ actionLancer = false;
+ etapeMatch++;
+ }
+ break;
+ }
+ case 11 : {
+ robot.poussette(2000);
+ typeMovement = DEPLACEMENT_ARRIERE;
+ robot.marche_arriere(-5000);
+ etapeMatch++;
+ break;
+ }
+ case 12 : /* On se place devant la case bleu, on pousse le palet qui y est devant puis on lache le bleu */
+ {
+ typeMovement = DEPLACEMENT_AVANT;
+ robot.va_au_point(70000,105000,180);
+ robot.ligne_droite_basique(10000);
+ etapeMatch++;
+ break;
+ }
+ case 13 :
+ {
+ if(actionLancer == false)
+ {
+ lacherVentouse4();
+ updateAndWriteScore('0', '2', '4');
+ actionLancer = true;
+ }
+ if(finActions)
+ {
+ actionLancer = false;
+ finActions = false;
+ etapeMatch++;
+ }
+ break;
+ }
+ case 14 : /* On se place devant la case verte et on lache le vert*/
+ {
+ typeMovement = DEPLACEMENT_ARRIERE;
+ robot.marche_arriere(-5000);
typeMovement = DEPLACEMENT_AVANT;
- robot.ligne_droite_basique(26500);
- robot.poussette(300);
- updateAndWriteScore('0', '4', '0');
+ robot.va_au_point(58000,83000,180);
+ robot.ligne_droite_basique(5000);
+ etapeMatch++;
+ break;
+ }
+ case 15 :
+ {
+ if(actionLancer == false)
+ {
+ lacherVentouse2();
+ updateAndWriteScore('0', '3', '0');
+ actionLancer = true;
+ }
+ if(finActions)
+ {
+ actionLancer = false;
+ finActions = false;
+ etapeMatch++;
+ }
+ break;
+ }
+ case 16 : /* On se place devant la case rouge et on lache le rouge*/
+ {
+ typeMovement = DEPLACEMENT_ARRIERE;
+ robot.marche_arriere(-5000);
+ typeMovement = DEPLACEMENT_AVANT;
+ robot.va_au_point(58000,45000,180);
+ robot.ligne_droite_basique(5000);
+ etapeMatch++;
+ break;
+ }
+ case 17 :
+ {
+ if(actionLancer == false)
+ {
+ //lacherVentouse1();
+ //lacherVentouse3();
+ stopPompe();
+ updateAndWriteScore('0', '4', '2');
+ actionLancer = true;
+ }
+ if(finActions)
+ {
+ actionLancer = false;
+ finActions = false;
+ etapeMatch++;
+ }
+ break;
+ }
+ case 18 :
+ {
+ typeMovement = DEPLACEMENT_ARRIERE;
+ robot.marche_arriere(-5000);
+ etapeMatch++;
+ break;
+ }
+ case 19 : /* On depose prend les 2 derniers palets du grand distributeur */
+ {
+ typeMovement = DEPLACEMENT_AVANT;
+ robot.va_au_point(85000,130000,90);
+ robot.ligne_droite_basique(5000);
etapeMatch++;
-
+ break;
+ }
+ case 20 :
+ {
+ if(actionLancer != true) {
+ prendre2Palets();
+ actionLancer = true;
+ }
+ if(finActions == true) {
+ finActions = false;
+ actionLancer = false;
+ etapeMatch++;
+ }
break;
}
+ case 21 :
+ {
+ typeMovement = DEPLACEMENT_ARRIERE;
+ robot.marche_arriere(-5000);
+ etapeMatch++;
+ break;
+ }
+ case 22 : /* On se place devant la case verte et on lache le vert*/
+ {
+ typeMovement = DEPLACEMENT_ARRIERE;
+ robot.marche_arriere(-5000);
+ typeMovement = DEPLACEMENT_AVANT;
+ robot.va_au_point(58000,83000,180);
+ robot.ligne_droite_basique(5000);
+ etapeMatch++;
+ break;
+ }
+ case 23 :
+ {
+ if(actionLancer == false)
+ {
+ lacherVentouse4();
+ updateAndWriteScore('0', '4', '8');
+ actionLancer = true;
+ }
+ if(finActions)
+ {
+ actionLancer = false;
+ finActions = false;
+ etapeMatch++;
+ }
+ break;
+ }
+ case 24 : /* On se place devant la case rouge et on lache le rouge*/
+ {
+ typeMovement = DEPLACEMENT_ARRIERE;
+ robot.marche_arriere(-5000);
+ typeMovement = DEPLACEMENT_AVANT;
+ robot.va_au_point(58000,45000,180);
+ robot.ligne_droite_basique(5000);
+ etapeMatch++;
+ break;
+ }
+ case 25 :
+ {
+ if(actionLancer == false)
+ {
+ //lacherVentouse3();
+ stopPompe();
+ updateAndWriteScore('0', '5', '4');
+ actionLancer = true;
+ }
+ if(finActions)
+ {
+ actionLancer = false;
+ finActions = false;
+ etapeMatch++;
+ }
+ break;
+ }
+/* case 26 :
+ {
+ if(actionLancer == false)
+ {
+ stopPompe();
+ actionLancer = true;
+ }
+ if(finActions)
+ {
+ actionLancer = false;
+ finActions = false;
+ etapeMatch++;
+ }
+ break;
+ }*/
+ case 26 : /* On laisse la place au robot2 */
+ {
+
+ typeMovement = DEPLACEMENT_AVANT;
+ robot.va_au_point(65000,130000,90);
+ etapeMatch++;
+ break;
+ }
default: {
robot.vitesse_nulle_D(0);
robot.vitesse_nulle_G(0);
motors_stop();
+ stopPompe();
+ stopPompe();
+ stopPompe();
}
}
}
@@ -208,10 +525,10 @@
robot.vitesse_nulle_D(0);
robot.vitesse_nulle_G(0);
motors_stop();
+ stopPompe();
while(1);
}
-
/*-------- Fin Strategie classique violet -----------------------------------------------*/
@@ -223,32 +540,339 @@
write(bufferScore);
Timer tempsMatch; // timer en seconde
int etapeMatch = 0;
+ initJaune();
while(demarreur.getState() == false) {
//on attend
//pc.printf("On attend le depart du Robot Thanos\n\r");
}
- tempsArretMvt.attach(&arretSystem, 100.0);
- initJaune();
- robot.initialisation();
+ tempsArretMvt.attach(&arretSystem, TEMPSMATCH);
+ //robot.initialisation();//check si modif
lancerTimerEcran();
tempsMatch.start();
- typeEvitement = ARRET;
+ //typeEvitement = ARRET;
+
while(tempsMatch <= TEMPSMATCH) {
switch (etapeMatch) {
- case 0 : { /* On active l'expérience */
+ case 0 : { /* on prend les 3 palets du petit distributeur */
+ typeMovement = DEPLACEMENT_AVANT;
+ typeEvitement = ARRET;
+ //robot.va_au_point(271800,178000,90);
+ robot.ligne_droite_basique(77000);
+ robot.poussette(2000);
+ robot.marche_arriere(-3000);
+ etapeMatch++;
+ break;
+ }
+ case 1: {
+ if(actionLancer != true) {
+ prendre3Palets();
+ actionLancer = true;
+ }
+ if(finActions == true) {
+ finActions = false;
+ actionLancer = false;
+ etapeMatch++;
+ }
+ break;
+ }
+ case 2 : { /* On se place entre la case rouge et verte pour poser le palet rouge */
+ robot.poussette(2000);
+ typeMovement = DEPLACEMENT_ARRIERE;
+ robot.marche_arriere(-50000);
+ robot.rotation_abs(-90);
+ typeMovement = DEPLACEMENT_AVANT;
+ //robot.va_au_point(277500,60000,-90);
+ robot.ligne_droite_basique(95000);
+ etapeMatch++;
+ break;
+ }
+ case 3 : {
+ if(actionLancer != true) {
+ lacherVentouse2();
+ actionLancer = true;
+ }
+ if(finActions == true) {
+ updateAndWriteScore('0', '0', '6');
+ finActions = false;
+ actionLancer = false;
+ etapeMatch++;
+ }
+ break;
+ }
+ case 4 : { /* On se place entre la case verte et bleue pour poser le palet vert */
+ typeMovement = DEPLACEMENT_ARRIERE;
+ wait(1);
+ robot.marche_arriere(-30000);
+ etapeMatch++;
+ break;
+ }
+ case 5 : {
+ if(actionLancer != true) {
+ lacherVentouse3();
+ actionLancer = true;
+ }
+ if(finActions == true) {
+ updateAndWriteScore('0', '1', '2');
+ finActions = false;
+ actionLancer = false;
+ etapeMatch++;
+ }
+ break;
+ }
+ case 6 : { /* On se place en bordure de la case bleue pour poser le palet bleu */
+ wait(1);
+ robot.marche_arriere(-30000);
+ etapeMatch++;
+ break;
+ }
+ case 7 : {
+ if(actionLancer != true) {
+ lacherVentouse4();
+ actionLancer = true;
+ }
+ if(finActions == true) {
+ updateAndWriteScore('0', '1', '8');
+ finActions = false;
+ actionLancer = false;
+ etapeMatch++;
+ }
+ break;
+ }
+ case 8 : {
+ wait(1);
+ robot.marche_arriere(-15000);
+ robot.rotation_abs(180);
+ robot.ligne_droite_basique(40000);
+ robot.rotation_abs(90);
+ robot.ligne_droite_basique(5000);
+ robot.poussette(2000);
+ robot.marche_arriere(-3000);
+ etapeMatch++; //a modifier après le match
+ break;
+ }
+ case 9 : { /* on est placé devant le grand distributeur pour prendre les 4 premiers palets */
+ if(actionLancer != true) {
+ prendre4Palets();
+ actionLancer = true;
+ }
+ if(finActions == true) {
+ finActions = false;
+ actionLancer = false;
+ etapeMatch++;
+ }
+ break;
+ }
+ case 10 : {
+ robot.poussette(3000);
+ robot.marche_arriere(-10000);
+ etapeMatch++;
+ break;
+ }
+ case 11 : {
+ //prise des 4
+ while(1);
+ robot.poussette(2000);
+ typeMovement = DEPLACEMENT_ARRIERE;
+ robot.marche_arriere(-5000);
+ etapeMatch++;
+ break;
+ }
+ case 12 : /* On se place devant la case bleu, on pousse le palet qui y est devant puis on lache le bleu */
+ {
+ typeMovement = DEPLACEMENT_AVANT;
+ robot.va_au_point(230000,105000,0);
+ robot.ligne_droite_basique(10000);
+ etapeMatch++;
+ }
+ case 13 :
+ {
+ if(actionLancer == false)
+ {
+ lacherVentouse1();
+ updateAndWriteScore('0', '2', '4');
+ actionLancer = true;
+ }
+ if(finActions)
+ {
+ actionLancer = false;
+ finActions = false;
+ etapeMatch++;
+ }
+ break;
+ }
+ case 14 : /* On se place devant la case verte et on lache le vert*/
+ {
+ typeMovement = DEPLACEMENT_ARRIERE;
+ robot.marche_arriere(-5000);
typeMovement = DEPLACEMENT_AVANT;
- robot.ligne_droite_basique(26500);
- robot.poussette(300);
- updateAndWriteScore('0', '4', '0');
+ robot.va_au_point(242000,83000,0);
+ robot.ligne_droite_basique(5000);
+ etapeMatch++;
+ break;
+ }
+ case 15 :
+ {
+ if(actionLancer == false)
+ {
+ lacherVentouse3();
+ updateAndWriteScore('0', '3', '0');
+ actionLancer = true;
+ }
+ if(finActions)
+ {
+ actionLancer = false;
+ finActions = false;
+ etapeMatch++;
+ }
+ break;
+ }
+ case 16 : /* On se place devant la case rouge et on lache le rouge*/
+ {
+ typeMovement = DEPLACEMENT_ARRIERE;
+ robot.marche_arriere(-5000);
+ typeMovement = DEPLACEMENT_AVANT;
+ robot.va_au_point(242000,45000,0);
+ robot.ligne_droite_basique(5000);
+ etapeMatch++;
+ break;
+ }
+ case 17 :
+ {
+ if(actionLancer == false)
+ {
+ //lacherVentouse2();
+ //lacherVentouse4();
+ stopPompe();
+ updateAndWriteScore('0', '4', '2');
+ actionLancer = true;
+ }
+ if(finActions)
+ {
+ actionLancer = false;
+ finActions = false;
+ etapeMatch++;
+ }
+ break;
+ }
+ case 18 :
+ {
+ typeMovement = DEPLACEMENT_ARRIERE;
+ robot.marche_arriere(-5000);
+ etapeMatch++;
+ break;
+ }
+ case 19 : /* On depose prend les 2 derniers palets du grand distributeur */
+ {
+ typeMovement = DEPLACEMENT_AVANT;
+ robot.va_au_point(215000,130000,90);
+ robot.ligne_droite_basique(5000);
etapeMatch++;
-
+ break;
+ }
+ case 20 :
+ {
+ if(actionLancer != true) {
+ prendre2Palets();
+ actionLancer = true;
+ }
+ if(finActions == true) {
+ finActions = false;
+ actionLancer = false;
+ etapeMatch++;
+ }
break;
}
+ case 21 :
+ {
+ typeMovement = DEPLACEMENT_ARRIERE;
+ robot.marche_arriere(-5000);
+ etapeMatch++;
+ break;
+ }
+ case 22 : /* On se place devant la case verte et on lache le vert*/
+ {
+ typeMovement = DEPLACEMENT_ARRIERE;
+ robot.marche_arriere(-5000);
+ typeMovement = DEPLACEMENT_AVANT;
+ robot.va_au_point(242000,83000,0);
+ robot.ligne_droite_basique(5000);
+ etapeMatch++;
+ break;
+ }
+ case 23 :
+ {
+ if(actionLancer == false)
+ {
+ lacherVentouse1();
+ updateAndWriteScore('0', '4', '8');
+ actionLancer = true;
+ }
+ if(finActions)
+ {
+ actionLancer = false;
+ finActions = false;
+ etapeMatch++;
+ }
+ break;
+ }
+ case 24 : /* On se place devant la case rouge et on lache le rouge*/
+ {
+ typeMovement = DEPLACEMENT_ARRIERE;
+ robot.marche_arriere(-5000);
+ typeMovement = DEPLACEMENT_AVANT;
+ robot.va_au_point(242000,45000,00);
+ robot.ligne_droite_basique(5000);
+ etapeMatch++;
+ break;
+ }
+ case 25 :
+ {
+ if(actionLancer == false)
+ {
+ //lacherVentouse2();
+ stopPompe();
+ updateAndWriteScore('0', '5', '4');
+ actionLancer = true;
+ }
+ if(finActions)
+ {
+ actionLancer = false;
+ finActions = false;
+ etapeMatch++;
+ }
+ break;
+ }
+ case 26 :
+ {
+ if(actionLancer == false)
+ {
+ stopPompe();
+ actionLancer = true;
+ }
+ if(finActions)
+ {
+ actionLancer = false;
+ finActions = false;
+ etapeMatch++;
+ }
+ break;
+ }
+ case 27 : /* On laisse la place au robot2 */
+ {
+
+ typeMovement = DEPLACEMENT_AVANT;
+ robot.va_au_point(235000,130000,90);
+ etapeMatch++;
+ break;
+ }
default: {
robot.vitesse_nulle_D(0);
robot.vitesse_nulle_G(0);
motors_stop();
+ stopPompe();
+ stopPompe();
+ stopPompe();
}
}
}
@@ -257,9 +881,9 @@
robot.vitesse_nulle_D(0);
robot.vitesse_nulle_G(0);
motors_stop();
+ stopPompe();
while(1);
}
-
/*--------- Fin Strategie classique jaune --------------------------------------------*/