Intersection detecteTypeIntersection()
{
Intersection intersectionType = inter_none;
if ( isCenter() && ( isLeft() || isRight() ))
{
// Permettra un meilleur redressement par la suit
enregistrerDernierCapteurActif( dernierCapteurActif,
dernierCapteurCoteActif );
// Teste si une ligne est détectée à gauche
// cas possibles: cross, tBase, tRight, lLeft.
if( isLeft() )
{
trouverTypeIntersectionPourUnCote( intersectionType,
capteur_gauche,
capteur_droit );
}
// Teste si une ligne est détectée à droite
// cas possibles: cross, tBase, tLeft, lRight.
else if( isRight() )
{
trouverTypeIntersectionPourUnCote( intersectionType,
capteur_droit,
capteur_gauche );
}
}
return intersectionType;
}
bool balayer(uint8_t vitesse , uint16_t tempsBalayage, Direction dir)
{
bool ligneDetectee = (isLeft() || isCenter() || isRight()) ;
while (!ligneDetectee && tempsBalayage > 0)
{
if (dir == direction_left)
pivoterAntiHoraire(vitesse);
else
pivoterHoraire(vitesse);
_delay_ms(1);
ligneDetectee = (isLeft() || isCenter() || isRight()) ;
tempsBalayage -- ;
}
arreterRobot();
enregistrerDernierCapteurActif(dernierCapteurActif, dernierCapteurCoteActif);
return ligneDetectee ;
}
void enregistrerDernierCapteurActif( Capteur &m_capteur = dernierCapteurActif,
Capteur &m_capteurCote = dernierCapteurCoteActif )
{
if(isRight() && !isLeft() && !isCenter())
{
m_capteur = capteur_droit ;
m_capteurCote = capteur_droit;
}
else if(!isRight() && isLeft() && !isCenter())
{
m_capteur = capteur_gauche ;
m_capteurCote = capteur_gauche;
}
else if(!isRight() && !isLeft() && isCenter())
{
//m_capteurCote ne change pas
m_capteur = capteur_centre ;
}
else if (isRight() && !isLeft() && isCenter()) { //droite et centre: on ne peut pas avoir gauche
if (m_capteur == capteur_gauche || m_capteur == capteur_aucun)
m_capteur = capteur_centre;
m_capteurCote = capteur_droit;
}
else if (!isRight() && isLeft() && isCenter()) { //gauche et centre: on ne peut pas avoir droite
if (m_capteur == capteur_droit || m_capteur == capteur_aucun)
m_capteur = capteur_centre;
m_capteurCote = capteur_gauche;
}
else if (!isRight() && !isLeft() && !isCenter()) { //aucun changement à l'état si on est dans l'état perdu
//m_capteur = m_capteur;
}
else if (isRight() && isLeft() && isCenter()){ //les trois sont allumés: on change seulement si "aucun"
if (m_capteur == capteur_aucun)
m_capteur = capteur_centre;
}
else { //les deux extrémités sont allumés, mais pas le centre -- cas très rare, possible lorsqu'on tourne
// et que les capteurs sont trop proches de l'intersection
//m_capteur = m_capteur
}
}
bool balayerZone(uint8_t vitesseMoteurDroit , uint8_t vitesseMoteurGauche , uint16_t rayonDeBalayage )
{
bool ligneTrouvee = ( isLeft() || isRight() || isCenter() );
if (!ligneTrouvee)
ligneTrouvee = balayer(vitesseMoteurDroit , rayonDeBalayage/2, direction_left);
if (!ligneTrouvee)
ligneTrouvee = balayer(vitesseMoteurGauche , rayonDeBalayage, direction_right);
/*
if (!ligneTrouvee)
{
while(!isCenter() && !isLeft() && !isRight())
reculerRobot(vitesseMoteurDroit , vitesseMoteurGauche); //ligneTrouvee demeure false
arreterRobot();
}
*/
enregistrerDernierCapteurActif(dernierCapteurActif, dernierCapteurCoteActif);
return ligneTrouvee ;
}
void tournerAtNextIntersection(uint8_t vitesse, Direction dir)
{
// on vérifie si on est présentement sur une ligne-trajet,
// comme dans un cross, ou tTight ou tLeft
switch (dir)
{
//on fait avancer le robot un peu pour que le pivot se fasse au bon endroit
if (isLeft() || isRight() || isCenter())
{
for (uint8_t i = 0; (i < 50) && (isLeft() || isRight() || isCenter()); i++)
{
suivreTrajet();
_delay_ms(1);
}
}
else
{
avancerRobot(VITESSEINTERSECTIONDROITE, VITESSEINTERSECTIONGAUCHE);
_delay_ms(50);
}
case direction_left:
// on vérifie si le capteur de gauche est déjà sur la ligne trajet.
// Si oui, il faut tourner while il sort de la ligne, entre dans la zone blanche
// puis arrête quand la ligne transversale est au milieu.
// Sinon, on est déjà dans la zone blanche et on saute la première étape.
tournerAGauche(vitesse);
if (isLeft())
while (isLeft());
while (!isLeft());
while(!isCenter());
break;
case direction_right:
//même chose que direction_left, mais à l'inverse
tournerADroite(vitesse);
if (isRight())
while (isRight());
while (!isRight());
while (!isCenter());
break;
case direction_reculer:
//on recule tant qu'on n'est pas sur une ligne
reculerRobot(vitesse, vitesse);
while (!isRight() || !isLeft() || !isCenter()) ;
case direction_continue: //nobreak
default:
break;
}
//on arrête le robot dans tous les cas.
//arreterRobot();
}
void suivreTrajet()
{
enregistrerDernierCapteurActif(dernierCapteurActif, dernierCapteurCoteActif);
if(isRight() && !isLeft() && !isCenter())
{
ajusterDroite(VITESSEMOTEURDROITE , VITESSEMOTEURGAUCHE , PENTEPOURTOURNER );
}
else if(!isRight() && isLeft() && !isCenter())
{
ajusterGauche(VITESSEMOTEURDROITE , VITESSEMOTEURGAUCHE , PENTEPOURTOURNER );
}
else if(!isRight() && !isLeft() && isCenter())
{
avancerRobot(VITESSEMOTEURDROITE , VITESSEMOTEURGAUCHE );
}
// Dans le cas ou le robot se trouve dans un etat perdu
else if(!isRight() && !isLeft() && !isCenter())
{
if(dernierCapteurActif == capteur_centre)
{
//Si on vient de la droite et qu'on vient de dépasser le centre, on compense en retournant vers la droite
if (dernierCapteurCoteActif == capteur_droit)
ajusterGauche(VITESSEMOTEURDROITE , VITESSEMOTEURGAUCHE, MOYENNE_PENTEPOURTOURNER );
//vice-versa
else if (dernierCapteurCoteActif == capteur_gauche)
ajusterDroite(VITESSEMOTEURDROITE , VITESSEMOTEURGAUCHE, MOYENNE_PENTEPOURTOURNER );
//si on connait pas le dernier côté, on continue tout droit
else
avancerRobot(VITESSEMOTEURDROITE , VITESSEMOTEURGAUCHE );
}
if(dernierCapteurActif == capteur_centre)
{
///@todo Fonction d'ajustement précise à décommenter si tout va bien
//
// // Sert à ajuster la direction du robot avant même d'actionner les senseurs de côté
// // en se basant sur la direction d'où vient le robot.
// //
// // Si on vient de la droite et qu'on vient de dépasser le centre, on compense en retournant vers la droite
// if (dernierCapteurCoteActif == capteur_droit)
// ajusterGauche(VITESSEMOTEURDROITE , VITESSEMOTEURGAUCHE, MOYENNE_PENTEPOURTOURNER );
// //vice-versa
// else if (dernierCapteurCoteActif == capteur_gauche)
// ajusterDroite(VITESSEMOTEURDROITE , VITESSEMOTEURGAUCHE, MOYENNE_PENTEPOURTOURNER );
// //si on connait pas le dernier côté, on continue tout droit
// else
avancerRobot(VITESSEMOTEURDROITE , VITESSEMOTEURGAUCHE );
}
else if(dernierCapteurActif == capteur_droit)
{
ajusterDroite(VITESSEMOTEURDROITE , VITESSEMOTEURGAUCHE, PENTEPOURTOURNER );
}
else if(dernierCapteurActif == capteur_gauche)
{
ajusterGauche(VITESSEMOTEURDROITE , VITESSEMOTEURGAUCHE, PENTEPOURTOURNER );
}
else //dernierCapteurActif == aucun
{
//pour détectage d'erreur ou bogue seulement. Ce cas ne devrait jamais se produire, sauf si on démarre le circuit sans voir la ligne
reculerRobot(VITESSEMOTEURDROITE , VITESSEMOTEURGAUCHE );
}
}
enregistrerDernierCapteurActif(dernierCapteurActif, dernierCapteurCoteActif);
}
void JoystickShield::processCallbacks() {
processEvents();
// Joystick Callbacks
if (isCenter() && centerCallback != NULL) {
centerCallback();
}
if (isUp() && upCallback != NULL) {
upCallback();
}
if (isRightUp() && rightUpCallback != NULL) {
rightUpCallback();
}
if (isRight() && rightCallback != NULL) {
rightCallback();
}
if (isRightDown() && rightDownCallback != NULL) {
rightDownCallback();
}
if (isDown() && downCallback != NULL) {
downCallback();
}
if (isLeftDown() && leftDownCallback != NULL) {
leftDownCallback();
}
if (isLeft() && leftCallback != NULL) {
leftCallback();
}
if (isLeftUp() && leftUpCallback != NULL) {
leftUpCallback();
}
// Button Callbacks
if (isJoystickButton() && jsButtonCallback != NULL) {
jsButtonCallback();
}
if (isUpButton() && upButtonCallback != NULL) {
upButtonCallback();
}
if (isRightButton() && rightButtonCallback != NULL) {
rightButtonCallback();
}
if (isDownButton() && downButtonCallback != NULL) {
downButtonCallback();
}
if (isLeftButton() && leftButtonCallback != NULL) {
leftButtonCallback();
}
}
