FRC_equipe1
/
FRC_2019
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: mbed
Diff: main.cpp
- Revision:
- 15:3d4543a6c100
- Parent:
- 14:1be2f691cbb4
- Child:
- 16:05665faaa489
- Child:
- 18:a7dc3a63d7eb
--- a/main.cpp Tue May 28 16:26:14 2019 +0000
+++ b/main.cpp Mon Jun 03 16:37:37 2019 +0000
@@ -2,23 +2,118 @@
#include "CAN_asser.h"
#include "Robot.h"
-void automate_testLasers(Robot&);
+void testLasers(Robot&);
void automate_testABalle(Robot&);
void automate_deplacement(Robot&);
-typedef enum{CHERCHE_BALLE,BALLE_GAUCHE,BALLE_DROITE,BALLE_STRAIGHT} type_etat_balle;
-type_etat_balle etat_balle = CHERCHE_BALLE;
+typedef enum{TURN_FAST,RETROUVE,GO_STRAIGHT} type_etat_deplacement;
+type_etat_deplacement etat_deplacement = TURN_FAST;
int main(void)
{
Robot robot;
+ robot.setSpeed(60,300);
+
+ while(1)
+ {
+ automate_testABalle(robot);
+
+ testLasers(robot);
+
+ if(!robot.aBalle())
+ automate_deplacement(robot);
+ }
+}
+
+void testLasers(Robot& robot)
+{
+ static float past_gauche, past_droit, actual_gauche, actual_droit;
+
+ actual_gauche = robot.getDist(Laser::Gauche);
+ actual_droit = robot.getDist(Laser::Droit);
+
+
+ if(actual_droit < past_droit-30 && etat_deplacement == TURN_FAST)
+ {
+ robot.stop();
+ etat_deplacement = RETROUVE;
+ robot.setSpeed(30,300);
+ }
+ else if(actual_gauche < past_gauche-30 && etat_deplacement == RETROUVE)
+ {
+ robot.stop();
+ etat_deplacement = GO_STRAIGHT;
+ robot.setSpeed(40,300);
+ }
+
+
+ past_gauche = actual_gauche;
+ past_droit = actual_droit;
+}
+
+void automate_testABalle(Robot& robot)
+{
+ typedef enum {ATTENTE,ABALLE} type_etat;
+ static type_etat etat = ATTENTE;
+
+ switch(etat)
+ {
+ case ATTENTE:
+ robot.gobe(25);
+ if(robot.aBalle())
+ {
+ dbug.printf("A balle\n\r");
+ robot.stop();
+ etat = ABALLE;
+ }
+ break;
+
+ case ABALLE:
+ if(robot.tourne(1800))
+ {
+ dbug.printf("Tourne\n\r");
+ robot.ejecte(60);
+ etat = ATTENTE;
+ etat_deplacement = TURN_FAST;
+ }
+ break;
+ }
+}
+
+void automate_deplacement(Robot& robot)
+{
+ switch(etat_deplacement)
+ {
+ case TURN_FAST:
+ robot.tourne();
+ break;
+
+ case RETROUVE:
+ robot.tourne(-450);
+ break;
+
+ case GO_STRAIGHT:
+ robot.avance();
+ break;
+ }
+}
+
+
+
+/*int main(void)
+{
+ Robot robot;
+
while(1)
{
- automate_testLasers(robot);
+ if(etat_deplacement != BALLE_STRAIGHT)
+ automate_testLasers(robot);
+
automate_testABalle(robot);
+
if(!robot.aBalle())
automate_deplacement(robot);
}
@@ -40,12 +135,14 @@
case ATTENTE:
if(actual_gauche < past_gauche-30)
{
+ dbug.printf("TURN_LEFT\n\r");
etat = TURN_LEFT;
robot.stop();
}
if(actual_droit < past_droit-30)
{
+ dbug.printf("TURN_RIGHT\n\r");
etat = TURN_RIGHT;
robot.stop();
}
@@ -53,16 +150,18 @@
case TURN_LEFT:
- etat_balle = BALLE_GAUCHE;
+ etat_deplacement = BALLE_GAUCHE;
if(actual_droit < past_droit-30)
{
+ dbug.printf("VOIT_BALLE\n\r");
etat = VOIT_BALLE;
robot.stop();
}
if(actual_gauche > past_gauche+30)
{
+ dbug.printf("TURN_RIGHT\n\r");
etat = TURN_RIGHT;
robot.stop();
}
@@ -70,16 +169,18 @@
case TURN_RIGHT:
- etat_balle = BALLE_DROITE;
+ etat_deplacement = BALLE_DROITE;
if(actual_gauche < past_gauche-30)
{
+ dbug.printf("VOIT_BALLE\n\r");
etat = VOIT_BALLE;
robot.stop();
}
if(actual_droit > past_droit+30)
{
+ dbug.printf("TURN_LEFT\n\r");
etat = TURN_LEFT;
robot.stop();
}
@@ -87,16 +188,18 @@
case VOIT_BALLE:
- etat_balle = BALLE_STRAIGHT;
+ etat_deplacement = BALLE_STRAIGHT;
if(actual_gauche > past_gauche+30)
{
+ dbug.printf("TURN_RIGHT\n\r");
etat = TURN_RIGHT;
robot.stop();
}
if(actual_droit > past_droit+30)
{
+ dbug.printf("TURN_LEFT\n\r");
etat = TURN_LEFT;
robot.stop();
}
@@ -115,43 +218,60 @@
switch(etat)
{
case ATTENTE:
- robot.gobe(40);
+ robot.gobe(25);
if(robot.aBalle())
{
- //robot.stop();
+ dbug.printf("A balle\n\r");
+ robot.stop();
etat = ABALLE;
- etat_balle = CHERCHE_BALLE;
+ etat_deplacement = CHERCHE_BALLE;
}
break;
case ABALLE:
if(robot.tourne(1800))
{
+ dbug.printf("Tourne\n\r");
robot.ejecte(60);
+ }
+ if(!robot.aBalle())
etat = ATTENTE;
- }
break;
}
}
void automate_deplacement(Robot& robot)
{
- switch(etat_balle)
+ switch(etat_deplacement)
{
case CHERCHE_BALLE:
- robot.tourne(100);
+ robot.tourne();
break;
case BALLE_GAUCHE:
- robot.tourne(-50);
+ robot.tourne(-30);
break;
case BALLE_DROITE:
- robot.tourne(50);
+ robot.tourne(30);
break;
case BALLE_STRAIGHT:
- robot.avance((short)robot.getDist(Laser::Gauche,"mm"));
- break;
+ robot.avance();
+ /*static float allerJusqua;
+
+ if(robot.getDist(Laser::Gauche,"mm") < robot.getDist(Laser::Droit,"mm"))
+ allerJusqua = 0.95*robot.getDist(Laser::Gauche,"mm");
+ else
+ allerJusqua = 0.95*robot.getDist(Laser::Droit,"mm");
+
+ if(allerJusqua > 2500)
+ allerJusqua = 2500;
+
+ while(!robot.avance((int)allerJusqua))
+ {
+ dbug.printf("%f\n\r", robot.getDist(Laser::Gauche,"mm"));
+ }*/
+ /*break;
}
-}
\ No newline at end of file
+}*/
\ No newline at end of file