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
main.cpp
- Committer:
- theolp
- Date:
- 2019-06-08
- Revision:
- 36:0a6cb92024c7
- Parent:
- 35:6c6321b97ae6
- Child:
- 37:65650aab8387
File content as of revision 36:0a6cb92024c7:
#include "mbed.h" #include "CAN_asser.h" #include "Robot.h" #include "math.h" bool automate_aveugle(Robot&, bool&); //automate de début de partie void automate_ejecte(Robot&, bool&); // void automate_testLasers(Robot&); // void automate_testABalle(Robot&, bool&); // void automate_deplacement(Robot&); // bool automate_fin_de_partie(Robot&); //automate crève ballon de fin de partie bool automate_arretUrgence(Robot&); //automate d'arrêt d'urgence au filet void automate_run(Robot&); //automate principale de partie typedef enum{TURN_FAST, TURN_FAST_GO, STRAIGHT, STRAIGHT_GO, CORRECT_GAUCHE, CORRECT_GAUCHE_GO} type_etat_deplacement; type_etat_deplacement etat_deplacement = TURN_FAST; int main(void) { Robot robot; while(1) automate_run(robot); } void automate_run(Robot& robot) { static bool ejecte = false; typedef enum{ATTENTE, RUN_AVEUGLE, RECHERCHE_BALLES, FIN_PARTIE} type_etat; static type_etat etat = ATTENTE; static Timer T_partie; if(T_partie.read() > 80.0f) etat = FIN_PARTIE; switch(etat) { case ATTENTE: if(!Robot::Jack) { T_partie.start(); etat = RECHERCHE_BALLES; } break; case RUN_AVEUGLE: if(automate_aveugle(robot, ejecte) && robot.GoToXYT(2000,2500,0)) etat = RECHERCHE_BALLES; automate_ejecte(robot, ejecte); break; case RECHERCHE_BALLES: if(!robot.aBalle()) automate_deplacement(robot); // Recherche de balles automate_testLasers(robot); // Gestion du lancer de balle automate_testABalle(robot, ejecte); //Ejecte ou non en fonction des commandes des autres automates automate_ejecte(robot, ejecte); break; case FIN_PARTIE: automate_fin_de_partie(robot); break; } } void automate_testLasers(Robot& robot) { typedef enum{RAS, SUSPICION, CONFIRMATION, VERIFICATION, CORRIGE, ABALLE, ATTENTE_VISION} type_etat; static type_etat etat = RAS; static const int distMurBalle = 20; static Timer T_suspi, T_arret, T_balle; static float droit[2] = { robot.getDist(Laser::Droit),robot.getDist(Laser::Droit) }, gauche[2] = { robot.getDist(Laser::Gauche),robot.getDist(Laser::Gauche) }, distBalle; droit[0] = robot.getDist(Laser::Droit); gauche[0] = robot.getDist(Laser::Gauche); if(robot.aBalle()) etat = ABALLE; switch(etat) { case RAS: if(etat_deplacement != TURN_FAST_GO) etat_deplacement = TURN_FAST; if( droit[1] > (droit[0] + distMurBalle) && droit[0] <= 100) { T_suspi.start(); etat = SUSPICION; dbug.printf("SUSPICION\n\r"); } break; case SUSPICION: if(T_suspi.read_ms() < 500) { if( gauche[1] > (gauche[0] + distMurBalle) ) { etat = CONFIRMATION; dbug.printf("CONFIRMATION\n\r"); T_suspi.stop(); T_suspi.reset(); } } else { T_suspi.stop(); T_suspi.reset(); etat = RAS; dbug.printf("RAS\n\r"); } break; case CONFIRMATION: distBalle = gauche[0]; robot.stop(); T_arret.start(); etat = VERIFICATION; dbug.printf("VERIFICATION\n\r"); break; case VERIFICATION: if(T_arret.read_ms() > 300) { T_arret.stop(); T_arret.reset(); if( fabs(gauche[0] - droit[0]) <= 5.5f && fabs(gauche[0]-distBalle) <= 6.0f) { if(etat_deplacement != STRAIGHT_GO) { etat_deplacement = STRAIGHT; dbug.printf("STRAIGHT\n\r"); } } else { etat_deplacement = CORRECT_GAUCHE; etat = CORRIGE; dbug.printf("CORRIGE\n\r"); } } break; case CORRIGE: if( droit[1] > (droit[0] + distMurBalle) ) { if(etat_deplacement != STRAIGHT_GO) { etat_deplacement = STRAIGHT; dbug.printf("STRAIGHT\n\r"); } } break; case ABALLE: if(!robot.aBalle()) { T_balle.start(); etat = ATTENTE_VISION; dbug.printf("ATTENTE_VISION\n\r"); etat_deplacement = TURN_FAST; } break; case ATTENTE_VISION: if(T_balle.read() > 1.0f) { T_balle.stop(); T_balle.reset(); etat = RAS; dbug.printf("RAS\n\r"); } break; } // Conversions past droit[1] = droit[0]; gauche[1] = gauche[0]; } void automate_testABalle(Robot& robot, bool& ejecte) { typedef enum {ATTENTE, ABALLE} type_etat; static type_etat etat = ATTENTE; switch(etat) { case ATTENTE: if(robot.aBalle()) { robot.stop(); etat = ABALLE; robot.setSpeed(150,800); //vitesse de rotation vers le terrain ennemi } break; case ABALLE: if( robot.tourne( -robot.pos(Robot::THETA) ) ) { ejecte = true; etat = ATTENTE; etat_deplacement = TURN_FAST; } break; } } void automate_deplacement(Robot& robot) { switch(etat_deplacement) { case TURN_FAST: robot.stop(); robot.setSpeed(90,450); etat_deplacement = TURN_FAST_GO; break; case TURN_FAST_GO: if(robot.immobile()) robot.tourne(); break; case STRAIGHT: robot.stop(); robot.setSpeed(150,800); etat_deplacement = STRAIGHT_GO; break; case STRAIGHT_GO: if(robot.immobile()) robot.avance( robot.getDist(Laser::Gauche,"mm") ); break; case CORRECT_GAUCHE: robot.stop(); robot.setSpeed(35,450); etat_deplacement = CORRECT_GAUCHE_GO; break; case CORRECT_GAUCHE_GO: if(robot.immobile()) robot.tourne(-100); break; } } bool automate_fin_de_partie(Robot& robot) { typedef enum{AVANCE, EXPLOSE_BALLON, FIN_PARTIE} type_etat; static type_etat etat = AVANCE; static Timer T_fin; switch(etat) { case AVANCE: if(robot.GoToXYT(robot.pos(Robot::X),3600,0)) etat = EXPLOSE_BALLON; break; case EXPLOSE_BALLON: if(robot.leveBras()) { robot.exploseBallon(); T_fin.start(); etat = FIN_PARTIE; } break; case FIN_PARTIE: if(T_fin.read() >= 2.0f && robot.baisseBras()) { robot.arreteDisquette(); robot.stopRouleau(); return true; } } return false; } bool automate_aveugle(Robot& robot, bool& ejecte) { typedef enum{STRAIGHT1, PREMIER_TOURNE_D, TOURNE_D, STRAIGHT2, TOURNE_G} type_etat; static type_etat etat = STRAIGHT1; static Timer T_ejecte; static int cptBalles = 0; if(cptBalles == 2) return true; switch(etat) { case STRAIGHT1: if(robot.avance(1850 - DIST_CENTRE)) { ejecte = true; etat = PREMIER_TOURNE_D; } break; case PREMIER_TOURNE_D: if(robot.tourne(ANGLE_DROIT)) etat = STRAIGHT2; break; case TOURNE_D: if(robot.tourne(ANGLE_DROIT)) etat = STRAIGHT2; break; case STRAIGHT2: if(robot.avance(1500)) etat = TOURNE_G; break; case TOURNE_G: if(robot.tourne(-ANGLE_DROIT)) { ejecte = true; cptBalles++; etat = TOURNE_D; } break; } return false; } void automate_ejecte(Robot& robot, bool& ejecte) { typedef enum{GOBE, EJECTE, ATTENTE} type_etat; static type_etat etat = GOBE; static Timer timer_ejecte; switch(etat) { case GOBE: robot.gobe(); if(ejecte) etat = EJECTE; break; case EJECTE: robot.ejecte(); timer_ejecte.start(); etat = ATTENTE; break; case ATTENTE: if( timer_ejecte >= 1.0f ){ timer_ejecte.stop(); timer_ejecte.reset(); ejecte = false; etat = GOBE; } break; } }