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:
- 16:05665faaa489
- Parent:
- 15:3d4543a6c100
- Child:
- 17:aae5361ddddf
--- a/main.cpp Mon Jun 03 16:37:37 2019 +0000
+++ b/main.cpp Tue Jun 04 08:49:01 2019 +0000
@@ -2,276 +2,42 @@
#include "CAN_asser.h"
#include "Robot.h"
-void testLasers(Robot&);
-void automate_testABalle(Robot&);
-void automate_deplacement(Robot&);
-
-
-typedef enum{TURN_FAST,RETROUVE,GO_STRAIGHT} type_etat_deplacement;
-type_etat_deplacement etat_deplacement = TURN_FAST;
-
+void automate_testDeplacement(Robot&);
int main(void)
{
Robot robot;
- robot.setSpeed(60,300);
+ robot.setSpeed(50,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;
+ automate_testDeplacement(robot);
}
-void automate_testABalle(Robot& robot)
+void automate_testDeplacement(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)
- {
- if(etat_deplacement != BALLE_STRAIGHT)
- automate_testLasers(robot);
-
- automate_testABalle(robot);
-
- if(!robot.aBalle())
- automate_deplacement(robot);
- }
-}
-
-
-void automate_testLasers(Robot& robot)
-{
- typedef enum {ATTENTE, TURN_RIGHT, TURN_LEFT, VOIT_BALLE} type_etat;
- static type_etat etat = ATTENTE;
-
- static float past_gauche, past_droit, actual_gauche, actual_droit;
-
- actual_gauche = robot.getDist(Laser::Gauche);
- actual_droit = robot.getDist(Laser::Droit);
+ typedef enum{AVANCE,TOURNE,GOTO,SARRETE} type_etat;
+ static type_etat etat = AVANCE;
switch(etat)
{
- 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();
- }
- break;
-
-
- case TURN_LEFT:
- 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();
- }
+ case AVANCE:
+ if(robot.avance(2000))
+ etat = TOURNE;
break;
-
- case TURN_RIGHT:
- 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();
- }
+ case TOURNE:
+ if(robot.tourne(450))
+ etat = GOTO;
break;
-
- case VOIT_BALLE:
- 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();
- }
- break;
- }
-
- 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;
- etat_deplacement = CHERCHE_BALLE;
- }
+ case GOTO:
+ BendRadius(30,90,0,0);
+ etat = SARRETE;
break;
- case ABALLE:
- if(robot.tourne(1800))
- {
- dbug.printf("Tourne\n\r");
- robot.ejecte(60);
- }
- if(!robot.aBalle())
- etat = ATTENTE;
+ case SARRETE:
break;
}
}
-
-void automate_deplacement(Robot& robot)
-{
- switch(etat_deplacement)
- {
- case CHERCHE_BALLE:
- robot.tourne();
- break;
-
- case BALLE_GAUCHE:
- robot.tourne(-30);
- break;
-
- case BALLE_DROITE:
- robot.tourne(30);
- break;
-
- case BALLE_STRAIGHT:
- 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