NXC_Projet_Knossos

Knossos_BG.c

/*

 * Projet Knossos

 * Résolution de labyrinthe

 *

 * Lucas Bourneuf

 * Mathieu Gaborit

 *

 * Janvier 2011

 * License : WTFPL

 * Version : 2.0

 */









// ================================

//     BIBLIOTHEQUES

// ================================



#include "lineleader.lib"











// ================================

//     DEFINES

// ================================



#define Motors OUT_BC // moteurs

#define Mot_d OUT_B   // mot. droit

#define Mot_g OUT_C   // mot. gauche

#define Couleur IN_1  // capteur RGB

#define Line IN_2     // capteur LineLeader

#define BP IN_4       // Bouton poussoir (arrêt d'urgence)

#define VITESSE 50    // Vitesse de croisière

#define TEMPORISATION 175 // temps avant la vérification des couleurs captées

#define Kp 100



// Les fonctions travaillent avec un tableau de 8 int

//  afin d'allèger le code source, nous utilisons une directive de prépro

//   pour spécifier la taille

#define SIZE 8

#define NB_CASES_BLEUES 5

#define CASES_TABL NB_CASES_BLEUES+1







// ================================

//     VARIABLES GLOBALES

// ================================



bool suivre_ligne = true;

bool robot_Fonctionne = true;

bool retour = false;

int vitesse_droite = VITESSE;

int vitesse_gauche = VITESSE;





// Variables pour le retour

int intersections[CASES_TABL] = {0};

int pos_intersections = 0;







// ================================

//     SHORT FUNCTIONS

// ================================



void decalage(){RotateMotor(Motors, VITESSE, 430);}

void quart_tour_d() {RotateMotorEx(Motors, 50, 570, 100, true, false);}

void demi_tour() { quart_tour_d(); quart_tour_d();}

void rotation_droite() { OnFwd(Mot_d, -45); OnFwd(Mot_g, 45); }

void rotation_gauche() { OnFwd(Mot_d, 45); OnFwd(Mot_g, -45); }









// ================================

//     PROTOTYPES

// ================================



void setup();                          // Initialisations globales

void ya_du_rouge();                    // Réaction en cas de lecture de rouge

void ya_du_bleu();                     // Réaction en cas de lecture de bleu

void ya_du_vert();                     // Réaction en cas de lecture de vert

void ya_du_jaune();                    // Réaction en cas de lecture de jaune

float getError(int &tab[]);            // Fonction maîtresse du PID.

float borne_vitesse(float vitesse);    // Corrige la valeur envoyée en arg() pour qu'elle corresponde à une vitesse valable







// ================================

//     INITIALISATIONS

// ================================



// fonction d'init générale

void setup(){

     SetSensorLowspeed(Line);

     SetSensorColorFull(Couleur);

     SetSensorTouch(BP);

     LL_PowerOff(Line); // Eteindre le capteur

     LL_PowerOn(Line);

}









// ================================

//     REACTIONS NOEUDS DE PARCOURS

// ================================



void ya_du_rouge(){

     Wait(TEMPORISATION);

     if(Sensor(Couleur) == INPUT_REDCOLOR){

         decalage();

         OnFwd(Mot_d, -45);

         OnFwd(Mot_g, 45);

         until(Sensor(Couleur) == INPUT_BLACKCOLOR);

         Off(Motors);

     }

     pos_intersections--; // Retour

}





void ya_du_bleu(){

     Wait(TEMPORISATION);

     if(Sensor(Couleur) == INPUT_BLUECOLOR){

        if (retour) {

           // 1 -> droite

           // 2 -> avant

           // 3 -> gauche

           decalage();

           if (intersections[pos_intersections] == 1){

                rotation_droite();

                Wait(300);

                rotation_droite();

                until(Sensor(Couleur) == INPUT_BLACKCOLOR);

           } else if (intersections[pos_intersections] == 3) {

                rotation_gauche();

                Wait(300);

                rotation_gauche();

                until(Sensor(Couleur) == INPUT_BLACKCOLOR);

           }

           pos_intersections--;

        } else {

            long tick = 0;

            intersections[pos_intersections]++; // Retour

            pos_intersections++;

            tick = CurrentTick();

            decalage();

            rotation_gauche();

            Wait(300);

            rotation_gauche();

            until(Sensor(Couleur) == INPUT_BLACKCOLOR);

            if ((CurrentTick() -tick)/1000 >3) {        // Au dela de 3s, le quart de tour n'est plus plausible

                intersections[pos_intersections-1]++; // Retour

                tick = CurrentTick();

                rotation_droite();

                Wait(300);

                rotation_droite();

                until(Sensor(Couleur) == INPUT_BLACKCOLOR);

                if ((CurrentTick() - tick)/1000>6){       // le quart de tour n'est plus plausible

                    intersections[pos_intersections-1]++; // Retour

                    rotation_droite();

                    Wait(300);

                    rotation_droite();

                    until(Sensor(Couleur) == INPUT_BLACKCOLOR);

                }

            }

            if (intersections[pos_intersections-1] >= 4){

               pos_intersections--;

               intersections[pos_intersections]=0;

               pos_intersections--;

            }

        }

     }

}





void ya_du_vert(){

     Wait(TEMPORISATION);

     if(Sensor(Couleur) == INPUT_GREENCOLOR){

          if (retour) {

             TextOut(0, LCD_LINE3, "YOUPI");

             Off(Motors);

             robot_Fonctionne = false;

             Wait(1000);

          } else {

             decalage();

             OnFwd(Mot_d, -45);

             OnFwd(Mot_g, 45);

             until(Sensor(Couleur) == INPUT_BLACKCOLOR);

             Off(Motors);

             pos_intersections--;

             retour = true;

          }

     }

}





void ya_du_jaune(){

    Wait(TEMPORISATION);

    if(Sensor(Couleur) == INPUT_YELLOWCOLOR){

        robot_Fonctionne = false;

    }

}





// ================================

//     PID (suivis de ligne)

// ================================



void invert_tab(int &tab[]){

     int i;

     //tab[0]-=20; // TRUANDAGE

     for (i = 0; i < SIZE; i++){

         tab[i] = 100 - tab[i];

     }

}



int max_pos(int &tab[]){

    int i;

    int max = tab[0]; // maximum courant

    int max_i = 0;    // indice du max

    for (i=1; i < SIZE; i++){

        if(tab[i] > max){

                  max = tab[i];

                  max_i = i;

        }

    }

    return max_i;

}



float mp(int &tab[], int pos){

      int pos_m = pos-1;

      int pos_p = pos+1;

      if (pos_m < 0){pos_m = 0;}

      if (pos_p > 0){pos_p = 7;}

      return ((pos_m*tab[pos_m] + pos*tab[pos] + pos_p*tab[pos_p])/(tab[pos_m]+tab[pos]+tab[pos_p]));

}



float rescale(int val){

      return (val-3.5)/3.5;

}





float getError(int &tab[]){

     invert_tab(tab);

     int max_index = max_pos(tab);

     return rescale(mp(tab, max_index));

}





float borne_vitesse(float vitesse){

      if(vitesse > 100)

           return 100;

      else if( vitesse < -100)

           return -100;

      return vitesse;

}











// ================================

//     TACHES MAITRESSES

// ================================



// gestion de couleur

task gestion_couleur_bp(){

     int couleur;

     while(true){

         couleur = Sensor(Couleur);

         if (!(couleur == INPUT_BLACKCOLOR) && !(couleur == INPUT_WHITECOLOR) && suivre_ligne){

             Off(Motors);

             suivre_ligne = false;

             switch (couleur) {

                   case INPUT_REDCOLOR:

                        ya_du_rouge();

                        break;



                   case INPUT_BLUECOLOR:

                        ya_du_bleu();

                        break;



                   case INPUT_YELLOWCOLOR:

                        ya_du_jaune();

                        break;



                   case INPUT_GREENCOLOR:

                        ya_du_vert();

                        break;



                   default: break;

            }

            suivre_ligne = true;

        }

    }

}







// tâche de suivi de ligne

task follow_line(){

     int line_tab[SIZE];

     float error;

     while (true){

           if(suivre_ligne){

                LL_ReadRaw(Line, line_tab);



                error = getError(line_tab);

                NumOut(0, LCD_LINE1, error, true);

                

                // --- DEBUG RETOUR ---

                TextOut(0, LCD_LINE3, "Tableau :");

                int i;

                for(i=0;i<NB_CASES_BLEUES;i++){

                     NumOut(10*i, LCD_LINE4, intersections[i]);

                }

                // --- END DEBUG ---



                OnFwd(Mot_d, borne_vitesse(VITESSE - error*Kp));

                OnFwd(Mot_g, borne_vitesse(VITESSE + error*Kp));

          }

     }

}







// main

task main(){

     setup();

     StartTask(follow_line);

     StartTask(gestion_couleur_bp);



     while(true){

         if(!robot_Fonctionne){

             Off(Motors);

             StopAllTasks();

         }

     }

}