Skip to content

Instantly share code, notes, and snippets.

@Aluriak

Aluriak/Fangio.c

Created December 23, 2011 17:12
Show Gist options
  • Save Aluriak/1514808 to your computer and use it in GitHub Desktop.
Save Aluriak/1514808 to your computer and use it in GitHub Desktop.
Download ZIP
Fangio NXC robot
Raw
Fangio.c
/*
!========================================================================================!
! Programme de Mathieu GABORIT et Lucas BOURNEUF !
! pour le projet FANGIO (séquence 3) !
! Licence : WTFPL !
! Etat : Terminal !
!========================================================================================!
//*/
// Problème de fluidité important sur le robot
// enlever la gestion du calcul du temps d'exécution pour voir si ça s'améliore.
//======================
// DEFINES
//======================
#define TV_d IN_1 // télémètre dirigé vers la droite (sert à déterminer la fin d'un obstacle que l'on esquive)
#define Couleur IN_2 // capteur couleur (vers le sol)
#define TVA IN_3 // télémètre dirigé vers l'avant (sert à locialiser les obstacles)
#define Touch IN_4 // capteur tactile (arrêt d'urgence)
#define trop_a_droite (couleur_vue == INPUT_REDCOLOR)
#define trop_a_gauche (couleur_vue == INPUT_GREENCOLOR)
#define marque (couleur_vue == INPUT_BLUECOLOR) // on voit une marque bleue
#define DISTANCE_CHEMIN_LIBRE 20 // distance à laquelle on considère que le chemin est libre
#define motors OUT_BC // moteurs branchés sur les ports B et C
#define NB_MARQUE_MAX 18 // nombre de marques bleues nécessaire pour finir le circuit
#define TEMPS_EMERGENCY_JAUNE 50 // temporisation après lecture de jaune, pour éviter des erreurs de lectures intempestives
#define VITESSE_DE_CROISIERE 65 // puissance des moteurs lors de parcours de piste
#define YA_UNE_LIGNE (couleur_vue == INPUT_BLACKCOLOR || couleur_vue == INPUT_REDCOLOR || couleur_vue == INPUT_GREENCOLOR) // vrai si on est sur une ligne (noir, rouge ou vert)
// contantes utilisées pour l'état d'esprit de la machine
#define SUIVRE_LIGNE 0
#define CHERCHE_LIGNE 1
#define ESQUIVE_OBSTACLE 2
//======================
// VARIABLES GLOBALES
//======================
int couleur_vue;
int derniere_couleur_vue = INPUT_BLACKCOLOR;
int compteur = 0; // compteur des marques
int Etat_Esprit = CHERCHE_LIGNE; // définit ce que fait le robot (utilisation des defines) (initialisé à cherche_ligne)
bool Robot_Fonctionne = true; // le main ne peut fonctionner que si il estvrai
unsigned long first_tick = 0; // utilisé pour sauvegarder le moment de démarrage du programme.
//======================
// PROTOTYPES
//======================
void setup(); // Initialisation et lancement du programme
task Emergency_stop(); // Tache gérant la fin de programme selon trois conditions non exclusives : bord de circuit, NB_MARQUE_MAX marques bleues ou bouton d'arrêt d'urgence
bool Presence_Obstacle_Droite(); // fonction renvoyant vrai tant que le télémètre dirigé vers la droite (TV_d) trouve un objet à moins de 20 cm.
bool Presence_Obstacle_Avant(); // Retourne vrai si un obstacle est détecté à moins de 5 cm du télémètre avant. (TVA°
void gestion_obstacle(); // gère l'esquive d'obstacle
//======================
// SHORT FONCTIONS
//======================
void quart_tour_d() { RotateMotorEx(motors, 100, 620, 100, true, false);} // faire un quart de tour par la droite
void quart_tour_g() { RotateMotorEx(motors, 100, 620, -100, true, false);} // faire un quart de tour par la gauche
void demi_tour() { // fait un demi tour par la droite
quart_tour_g();
quart_tour_g(); // deux quarts de tour, car lui faire faire un demi-tour avec RotateMotorEx() est imprécis
}
// ===========================
// Définitions des fonctions
// ===========================
void setup() // Initialisation et lancement du programme
{
SetSensorLowspeed(TV_d); // port du télémètre orienté à droite (étudier la possibilité de revenir sur un obstacle)
SetSensorColorFull(Couleur); // port couleur (voir la piste)
SetSensorLowspeed(TVA); // port télémètre orienté en avant (voir les obstacles avant de s'écraser dessus)
SetSensorTouch(Touch); // port tactile (arrêt d'urgence)
StartTask(Emergency_stop); // tache de gestion de fin de prgm (couleur jaune, compteur de marques bleues, arrêt d'urgence)
first_tick = FirstTick(); // variable enregistrant le moment de départ du programme.
} // end setup()
// Tache gérant la fin de programme selon trois conditions non exclusives : bord de circuit, NB_MARQUE_MAX marques bleues ou bouton d'arrêt d'urgence
task Emergency_stop()
{
bool nuitDesTemps = 0;
while(!nuitDesTemps) // jusqu'à la nuit la nuit des temps
{
// si le capteur couleur voit du jaune, s'il à compté 18 marques ou plus, ou si le bouton est pressé
if((couleur_vue == INPUT_YELLOWCOLOR))
{
Wait(TEMPS_EMERGENCY_JAUNE); // protection pour éviter de s'arrêter si le capteur détecte du jaune par accident
if(Sensor(Couleur) == INPUT_YELLOWCOLOR)
{
Robot_Fonctionne = false; // le main arrête de tourner.
Off(motors); // arrêt des moteurs
TextOut(10, LCD_LINE2, "Temps de");
TextOut(20, LCD_LINE3, "parcours : ");
TextOut(30, LCD_LINE4, NumToStr((first_tick - CurrentTick())/1000000000)); // on affiche (moment DémarragePrgm - MomentActuel) (c'est égal au temps d'exécution de prgm)
Wait(5000); // attendre cinq secondes, qu'on puisse voir les résultats.
StopAllTasks(); // arrêt du programme
}
}
else if(compteur >= NB_MARQUE_MAX || Sensor(Touch)) // si on a égalé ou dépassé le nombre de marque bleues maximum, on s'arrête. De même si on enclenche le capteur tactile.
{
Robot_Fonctionne = false; // le main arrête de tourner.
Off(motors); // arrêt des moteurs
TextOut(10, LCD_LINE2, "Temps de");
TextOut(20, LCD_LINE3, "parcours : ");
TextOut(30, LCD_LINE4, NumToStr((first_tick - CurrentTick())/1000000000)); // on affiche (moment DémarragePrgm - MomentActuel) (c'est égal au temps d'exécution de prgm)
Wait(5000); // attendre cinq secondes, qu'on puisse voir les résultats.
StopAllTasks(); // arrêt du programme
}
}
}// end Emergency_stop()
// fonction renvoyant vrai tant que le télémètre dirigé vers la droite trouve un objet à moins de 20 cm.
bool Presence_Obstacle_Droite()
{
if(SensorUS(TV_d) <= DISTANCE_CHEMIN_LIBRE)
return true;
return false;
}
// retourne vrai si le télémètre de devant trouve un obstacle à moins de 5 centimètres.
bool Presence_Obstacle_Avant()
{
if(SensorUS(TVA) <= 5)
return true;
return false;
}
// gère l'esquive d'obstacle
void gestion_obstacle()
{
RotateMotor(motors, VITESSE_DE_CROISIERE, -180);
quart_tour_g();
OnFwd(motors, 50);
while(Presence_Obstacle_Droite()){
couleur_vue = Sensor(Couleur);
if(YA_UNE_LIGNE)
{
Etat_Esprit = CHERCHE_LIGNE;
return;
}
} // end boucle while
RotateMotor(motors, VITESSE_DE_CROISIERE, 400);
quart_tour_d();
RotateMotor(motors, VITESSE_DE_CROISIERE, 400);
OnFwd(motors, 50);
while(Presence_Obstacle_Droite()){
couleur_vue = Sensor(Couleur);
if(YA_UNE_LIGNE)
{
Etat_Esprit = CHERCHE_LIGNE;
return;
}
} // end boucle while
RotateMotor(motors, VITESSE_DE_CROISIERE, 400);
quart_tour_d();
Etat_Esprit = CHERCHE_LIGNE;
}
task main() // main(), l'épine dorsale du prgm
{
setup(); // intialisation
int left_rotate; // vars contenant l'angle de rotation
int right_rotate; // ..
bool boucle = true; // dans le cas où on fait des boucles, on a un boléen tout prêt.
// boucle principale de programme. Etudie les différentes couleurs
bool semaineDesQuatreJeudis = false;
while(!semaineDesQuatreJeudis)
{
couleur_vue = Sensor(Couleur); // Pour éviter de tester en permanence le capteur couleur, on utilisera une variable actualisée à chaque tour de boucle.
// on teste la présence d'un obstacle avant la gestion des couleurs
if(Presence_Obstacle_Avant())
{
gestion_obstacle();
}
else if(!Robot_Fonctionne)
{
Off(motors);
}
else
{
// switch maître sur la variable couleur_vue
switch (couleur_vue)
{
case INPUT_REDCOLOR: // ROUGE
if(derniere_couleur_vue == INPUT_REDCOLOR) left_rotate =40;
else if(derniere_couleur_vue == INPUT_WHITECOLOR)
left_rotate = 180;
else left_rotate = 80;
RotateMotorEx(motors, 80, left_rotate, -50, true, false);
break;
case INPUT_GREENCOLOR: // VERT
if(derniere_couleur_vue == INPUT_GREENCOLOR) right_rotate =40;
else if(derniere_couleur_vue == INPUT_WHITECOLOR)
right_rotate = 180;
else right_rotate = 80;
RotateMotorEx(motors, 80, right_rotate, 50, true, false);
break;
case INPUT_BLUECOLOR: // BLEU
if (couleur_vue != derniere_couleur_vue && Etat_Esprit == SUIVRE_LIGNE)
{
Wait(5);
if(Sensor(Couleur) == couleur_vue)
{
compteur++;
TextOut(25,LCD_LINE2, NumToStr(compteur));
}
}
OnFwd(motors,70);
break;
case INPUT_YELLOWCOLOR: // JAUNE
Wait(TEMPS_EMERGENCY_JAUNE); // protection pour éviter de s'arrêter si le capteur détecte du jaune par accident
if(Sensor(Couleur) == INPUT_YELLOWCOLOR)
Off(motors);
break;
case INPUT_WHITECOLOR: // BLANC
Etat_Esprit = CHERCHE_LIGNE;
Wait(10);
if(Sensor(Couleur) == INPUT_WHITECOLOR)
{
// recherche préliminaire de ligne, dans les zones proches, histoire de pas déclencher l'arme nucléaire pour invasion de mouches
RotateMotorEx(motors, 100, 60, 100, true, true); // tourner sur quelques centimètres à droite
couleur_vue = Sensor(Couleur);
if(YA_UNE_LIGNE) {
Etat_Esprit = SUIVRE_LIGNE;
break;
}
RotateMotorEx(motors, 100, 120, -100, true, true); // tourner sur quelques centimètres à gauche
couleur_vue = Sensor(Couleur);
if(YA_UNE_LIGNE) {
Etat_Esprit = SUIVRE_LIGNE;
break;
}
RotateMotorEx(motors, 100, 60, 100, true, true); // tourne pour revenir dans l'axe
couleur_vue = Sensor(Couleur);
if(YA_UNE_LIGNE) {
Etat_Esprit = SUIVRE_LIGNE;
break;
}
// fin de recherche préliminaire
// on avance jusqu'à trouver une ligne
OnFwd(motors, 100);
while(!YA_UNE_LIGNE)
{couleur_vue = Sensor(Couleur);}
RotateMotorEx(motors, 100, 140, 0, false, true); // parcourir quelques centimètres
while(couleur_vue != INPUT_BLACKCOLOR && couleur_vue != INPUT_BLUECOLOR) // si ya pas de ligne noire (ou bleue)
{
couleur_vue = Sensor(Couleur);
//*
if(couleur_vue == INPUT_WHITECOLOR || couleur_vue == INPUT_REDCOLOR)
RotateMotorEx(motors, 75, 10, -100, true, false);
if(couleur_vue == INPUT_GREENCOLOR)
{
//*
RotateMotorEx(motors, 100, 60, 100, true, true); // on le décale vers la droite
couleur_vue = Sensor(Couleur);
if(YA_UNE_LIGNE)
{
Etat_Esprit = SUIVRE_LIGNE; // on a trouvé une ligne !
OnFwd(motors,VITESSE_DE_CROISIERE); // on continue l'exploration
break;
}
else // si c'est du blanc, c'est qu'on est pas dans le bon sens... donc on se retourne ! //*/
demi_tour(); // on fait un demi tour pour dépasser la ligne sans passer par la ligne jaune
}
}
}
Etat_Esprit = SUIVRE_LIGNE; // on a trouvé une ligne !
OnFwd(motors,VITESSE_DE_CROISIERE);
break;
case INPUT_BLACKCOLOR: // NOIR
OnFwd(motors,VITESSE_DE_CROISIERE);
default: OnFwd(motors,VITESSE_DE_CROISIERE);
}
derniere_couleur_vue = couleur_vue;
} // end conditions de gestion d'obstacle
} // end boucle principale
} // end main()
// fin du prgm
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment