From 5a3126f7926b8b7df0eec571a59958a97c63084c Mon Sep 17 00:00:00 2001 From: Victor Dubois Date: Sun, 13 May 2012 22:53:38 +0200 Subject: [PATCH] derniere version suiveur de ligne uniquement --- petit_robot/Capteur.cpp | 32 + petit_robot/Capteur.h | 24 + petit_robot/Detection.cpp | 149 +++ petit_robot/Detection.h | 36 + petit_robot/Diode.cpp | 41 + petit_robot/Diode.h | 22 + ...t_1_7_3_bouteille2_okviolet_3_malloc_6.ino | 934 ++++++++++++++++++ petit_robot/Sharp.cpp | 41 + petit_robot/Sharp.h | 23 + petit_robot/Ultrason.cpp | 70 ++ petit_robot/Ultrason.h | 24 + 11 files changed, 1396 insertions(+) create mode 100644 petit_robot/Capteur.cpp create mode 100644 petit_robot/Capteur.h create mode 100644 petit_robot/Detection.cpp create mode 100644 petit_robot/Detection.h create mode 100644 petit_robot/Diode.cpp create mode 100644 petit_robot/Diode.h create mode 100644 petit_robot/Petit_robot_1_7_3_bouteille2_okviolet_3_malloc_6.ino create mode 100644 petit_robot/Sharp.cpp create mode 100644 petit_robot/Sharp.h create mode 100644 petit_robot/Ultrason.cpp create mode 100644 petit_robot/Ultrason.h diff --git a/petit_robot/Capteur.cpp b/petit_robot/Capteur.cpp new file mode 100644 index 00000000..2eec77d6 --- /dev/null +++ b/petit_robot/Capteur.cpp @@ -0,0 +1,32 @@ +#include "Capteur.h" +#include "Arduino.h" +#include "Sharp.h" +#include "Ultrason.h" + +Capteur::Capteur(int capteurPin) : capteurPin(capteurPin), valeur(0) +{ + //capteurPin = capteurPin; + //valeur = 0; +} + +Capteur::Capteur(int capteurPin, int valeur) : capteurPin(capteurPin), valeur(valeur) +{ + //capteurPin = capteurPin; + //valeur = 0; +} + +Capteur::~Capteur() +{ + +} + +int Capteur::GetValue() +{ + Update(); + return valeur; +} + +void Capteur::Update() +{ + +} diff --git a/petit_robot/Capteur.h b/petit_robot/Capteur.h new file mode 100644 index 00000000..07814246 --- /dev/null +++ b/petit_robot/Capteur.h @@ -0,0 +1,24 @@ +#ifndef DEF_CAPTEUR // Si la constante n'a pas été définie` le fichier n'a jamais été inclus +#define DEF_CAPTEUR // On définit la constante pour que la prochaine fois le fichier ne soit plus inclus + + + +class Capteur +{ + public: + + Capteur(int capteurPin); + Capteur(int capteurPin, int valeur); + ~Capteur(); + int GetValue(); + void Update(); + + private: + + protected://Les capteurs sharp peuvent modifier leurs "valeur". + + int valeur; //Distance ou autre, la valeur renvoyée par le capteur. + int capteurPin; +}; + +#endif diff --git a/petit_robot/Detection.cpp b/petit_robot/Detection.cpp new file mode 100644 index 00000000..d53688ec --- /dev/null +++ b/petit_robot/Detection.cpp @@ -0,0 +1,149 @@ +#include "Detection.h" +#include "Arduino.h" +#include "Capteur.h" +#include "Ultrason.h" +#include "Sharp.h" + +//enum sharpDetectant { AUCUN, GAUCHE, DROITE, TTAVANT, ARRIERE, TTGAUCHE, TTDROITE, TT, AUCUNETHAUT, GAUCHEETHAUT, DROITEETHAUT, TTAVANTETHAUT, ARRIEREETHAUT, TTGAUCHEETHAUT, TTDROITEETHAUT, TTETHAUT, AUCUNETMILIEU, GAUCHEETMILIEU, DROITEETMILIEU, TTAVANTETMILIEU, ARRIEREETMILIEU, TTGAUCHEETMILIEU, TTDROITEETMILIEU, TTETMILIEU, AUCUNETHAUTETMILIEU, GAUCHEETHAUTETMILIEU, DROITEETHAUTETMILIEU, TTAVANTETHAUTETMILIEU, ARRIEREETHAUTETMILIEU, TTGAUCHEETHAUTETMILIEU, TTDROITEETHAUTETMILIEU, TTETHAUTETMILIEU}; + +//enum sharpDetectant { AUCUN, GAUCHE, DROITE, TTAVANT, ARRIERE, TTGAUCHE, TTDROITE, TT }; + +enum numeroSharp { GAUCHE, DROITE, MILIEU, HAUT, ARRIERE }; + +enum sens { AVANCER, RECULER, TOURNEGAUCHE, TOURNEDROITE, AVANCERBOUTEILLE }; +/*#define pinSharpAvG A6 +#define pinSharpAvD A4 +#define pinSharpArr A7 +#define pinUltrason 10*/ +#define seuilSharpAvG 350 +#define seuilSharpAvD 350 +#define seuilSharpMilieu 350 +#define seuilSharpHaut 350 +#define seuilSharpArr 350 + + +Detection::Detection() +{ + for (int initcvqqch = 0 ; initcvqqchVoitQQch()) + { + capteurVoitQQch[iLireCapteurs]--;//On décrémente si on ne voit rien. + } + else + { + capteurVoitQQch[iLireCapteurs]=8;//Sinon on remet le compteur à 8. + } + + if(capteurVoitQQch[iLireCapteurs]==0) + { + capteurVoitObstacle[iLireCapteurs]=0; + } + } + else//Si on n'est pas devant un obstacle + { + if(capteurSharp[iLireCapteurs]->VoitQQch()) + { + capteurVoitQQch[iLireCapteurs]++;//On décrémente si on ne voit rien. + } + else + { + capteurVoitQQch[iLireCapteurs]=0;//Sinon on remet le compteur à 8. + } + + if(capteurVoitQQch[iLireCapteurs]==8) + { + capteurVoitObstacle[iLireCapteurs]=1; + } + } + } + /* + capteurVoitQQch[GAUCHE]=min(8,(capteurVoitQQch[GAUCHE]+1)*capteurAvantG->VoitQQch()); + + capteurVoitQQch[DROITE]=min(8,(capteurVoitQQch[DROITE]+1)*capteurAvantD->VoitQQch()); + + capteurVoitQQch[MILIEU]=min(8,(capteurVoitQQch[MILIEU]+1)*capteurMilieu->VoitQQch()); + + capteurVoitQQch[HAUT]=min(8,(capteurVoitQQch[MILIEU]+1)*capteurHaut->VoitQQch()); + + capteurVoitQQch[ARRIERE]=min(8,(capteurVoitQQch[ARRIERE]+1)*capteurArriere->VoitQQch());*/ + + /*capteurVoitQQch[GAUCHE]=capteurAvantG->VoitQQch(); + + capteurVoitQQch[DROITE]=capteurAvantD->VoitQQch(); + + capteurVoitQQch[MILIEU]=capteurMilieu->VoitQQch(); + + capteurVoitQQch[HAUT]=capteurHaut->VoitQQch(); + + capteurVoitQQch[ARRIERE]=capteurArriere->VoitQQch();*/ + +} diff --git a/petit_robot/Detection.h b/petit_robot/Detection.h new file mode 100644 index 00000000..0c61e761 --- /dev/null +++ b/petit_robot/Detection.h @@ -0,0 +1,36 @@ +#ifndef DEF_DETECTION // Si la constante n'a pas été définie` le fichier n'a jamais été inclus +#define DEF_DETECTION // On définit la constante pour que la prochaine fois le fichier ne soit plus inclus + +#include "Sharp.h" +#include "Ultrason.h" + +#define NOMBRECAPTEUR 5 + + +class Detection +{ + public: + + Detection(); + ~Detection(); + int JePeuxAvancer(int sensVoulu); + void LireCapteursDistance(); + + private: + + + int capteurVoitQQch[NOMBRECAPTEUR]; + int capteurVoitObstacle[NOMBRECAPTEUR];//tableau permettant de pallier le problème du bruit : il faut capter 8 fois d'affillée pour voir un obstacle + //Ultrason ping; + /*Sharp *capteurAvantG; + Sharp *capteurAvantD; + Sharp *capteurMilieu; + Sharp *capteurHaut; + Sharp *capteurArriere;*/ + Sharp **capteurSharp; + + +}; + +#endif + diff --git a/petit_robot/Diode.cpp b/petit_robot/Diode.cpp new file mode 100644 index 00000000..2290d9b4 --- /dev/null +++ b/petit_robot/Diode.cpp @@ -0,0 +1,41 @@ +#include "Diode.h" +#include "Arduino.h" + +//using namespace std; + +Diode::Diode(int diodePin)//Constructeur +{ + int duree = 0; + diodePin = diodePin; +} + +Diode::~Diode()//Destructeur +{ + +} + +//On éteind les diodes qui doivent l'etre +//permet d'avoir des diodes allumées pendant des cycles entiers, pour qu'elles soient suffisament lumineuses. +void Diode::DiodeOff() +{ + + if(dureeEncoreAllumee>1) + { + dureeEncoreAllumee--; + } + else + { + if(dureeEncoreAllumee==1) + { + digitalWrite(diodePin,LOW); + dureeEncoreAllumee--; + } + } +} + +void Diode::DiodeOn(int duree) +{ + dureeEncoreAllumee=duree; + digitalWrite(diodePin,HIGH); +} + diff --git a/petit_robot/Diode.h b/petit_robot/Diode.h new file mode 100644 index 00000000..a1c8ae42 --- /dev/null +++ b/petit_robot/Diode.h @@ -0,0 +1,22 @@ +#ifndef DEF_DIODE // Si la constante n'a pas été définie` le fichier n'a jamais été inclus +#define DEF_DIODE // On définit la constante pour que la prochaine fois le fichier ne soit plus inclus + + +class Diode +{ + public: + + Diode(int diodePin); + ~Diode(); + void DiodeOff();//eteind une diode + void DiodeOn(int duree);//allume une diode + + private: + + int dureeEncoreAllumee;//Correspond au nombre de cycles pendant lesquels la diode va continuer à etre allumée. + int diodePin;//le pin de l'arduino auquel est attché la diode. + +}; + +#endif + diff --git a/petit_robot/Petit_robot_1_7_3_bouteille2_okviolet_3_malloc_6.ino b/petit_robot/Petit_robot_1_7_3_bouteille2_okviolet_3_malloc_6.ino new file mode 100644 index 00000000..8527ffcb --- /dev/null +++ b/petit_robot/Petit_robot_1_7_3_bouteille2_okviolet_3_malloc_6.ino @@ -0,0 +1,934 @@ +#include +#include "Diode.h" +#include "Detection.h" +#include + +#define AUTO 0 +#define RATTRAPAGE 1 + + + // DEFINITION DES CONSTANTES // + +const int pinRoueG = 5; +const int pinRoueD = 3; + + + +const int pinCalClaire = 9; +const int pinCalObs = 7; +const int pinCalGD = 6; + +//##### DFRduino nano +/*const int pinCCG = A0;//pin du CapteurCouleurGauche +const int pinCCD = A1; + +const int pinTirr = A5; + +const int pinSharpAvG = A6; +const int pinSharpAvD = A4; +const int pinSharpArr = A7;*/ + +//##### Arduino nano +const int pinCCG = A0;//pin du CapteurCouleurGauche +const int pinCCD = A7; + +const int pinTirr = A2; + +const int pinSharpAvG = A1;//attention, aussi défini dans detection +const int pinSharpAvD = A3; +const int pinSharpArr = A6; + +const int pinLed1 = 8; +const int pinLed2 = 11; +const int pinLed3 = 12; + +const int pinPing = 10; + +enum couleurMatch { ROUGE, VIOLET }; +enum sharpDetectant { AUCUN, GAUCHE, DROITE, TTAVANT, ARRIERE, TTGAUCHE, TTDROITE, TT, AUCUNETHAUT, GAUCHEETHAUT, DROITEETHAUT, TTAVANTETHAUT, ARRIEREETHAUT, TTGAUCHEETHAUT, TTDROITEETHAUT, TTETHAUT, AUCUNETMILIEU, GAUCHEETMILIEU, DROITEETMILIEU, TTAVANTETMILIEU, ARRIEREETMILIEU, TTGAUCHEETMILIEU, TTDROITEETMILIEU, TTETMILIEU, AUCUNETHAUTETMILIEU, GAUCHEETHAUTETMILIEU, DROITEETHAUTETMILIEU, TTAVANTETHAUTETMILIEU, ARRIEREETHAUTETMILIEU, TTGAUCHEETHAUTETMILIEU, TTDROITEETHAUTETMILIEU, TTETHAUTETMILIEU}; +enum sens { AVANCER, RECULER, TOURNEGAUCHE, TOURNEDROITE, AVANCERBOUTEILLE }; + +int couleurRobot = 0; + + // PARAMETRES DE DEPLACEMENT AUTOMATIQUE // + +const unsigned long dureeMatch = 89000;// 89 secondes : mieux vaut ne pas prendre de risques +//const unsigned long dureeMatch = 9000;// 89 secondes : mieux vaut ne pas prendre de risques +unsigned long tempDepartMatch = 0; + +const int paramArretRoueG = 90; +const int paramArretRoueD = 90; + + + const int TTDroitRoueG = 110; +const int TTDroitRoueD = 69;//70à + +const int TournerGaucheRoueG = 82; +const int TournerGaucheRoueD = 82; +const int TournerGaucheTemps = 865; + +const int TournerDroiteRoueG = 98; +const int TournerDroiteRoueD = 98; +const int TournerDroiteTemps = 865; + +const int ReculerRoueG = 77; +const int ReculerRoueD = 110; + +const int ReculerLentRoueG = 85; +const int ReculerLentRoueD = 95; + + + + + + + +const unsigned long tempsMaxCapteurs = 10; + + // PARAMETRES DE SUIVEUR DE LIGNE // + +const int valMoyenneG = 0; +const int valMoyenneD = 0; + +//pour lancer le rattrapa(ge +int seuilHautRattrapageG = 0; +int seuilHautRattrapageD = 0; +int seuilBasRattrapageG = 0; +int seuilBasRattrapageD = 0; + +int basGauche=0; +int basDroite=0; +int hautGauche=0; +int hautDroite=0; + +int seuilHautDifferenceRattrapageG=0; +int seuilHautDifferenceRattrapageD=0; + +const int paramDiff = 2; + +const int paramD = 0; +const int paramG = 0; + +const int offsetD = 0; +const int offsetG = 0; + +const float paramSeuilHautRattrapage = 0.99; +const float paramSeuilBasRattrapage = 1.003; + + +//pour arreter le rattrppage +const int tempMaxRattrappage = 1000; + +int seuilHautRattrapeeG = 0; +int seuilHautRattrapeeD = 0; +int seuilBasRattrapeeG = 0; +int seuilBasRattrapeeD = 0; + +const float paramSeuilHautRattrapee = 0.8; +const float paramSeuilBasRattrapee = 1.1; + + // PARAMETRES DES SHARPS // +/* +const int seuilSharpAvG = 200; +const int seuilSharpAvD = 200; +const int seuilSharpArr = 200;*/ + +const int distanceMin = 15; + + + + + + // DEFINITION DES VARIBABLES // + +Servo myservoRoueGauche; +Servo myservoRoueDroite; + +int bouteilleUneAPousser=0; + + + // VARIABLES DIODES // + Diode diode1(pinLed1), diode2(pinLed2), diode3(pinLed3); + Detection detection; + + + +int cycleAllumeDiode1 = 0;//Nombre de cycle de boucle pendant lesquels la diode 1 doit etre allumée : permet d'avoir des diodes allumées pendant des cycles entiers, pour qu'elles soient suffisament lumineuses. +int cycleAllumeDiode2 = 0; +int cycleAllumeDiode3 = 0; + + // PROTOPTYPES DES FONCTIONS // + + void SuivreLigne(); + int DeplacementAuto(char* dir, unsigned long duree, int modeRattrappageLigne); + void Rattrapage(char* cote); + int LireCapteursSharp(); + void Arret(); + long microsecondsToCentimeters(long microseconds); + int DetectionUltrason(); + int JePeuxAvancer(int sensVoulu); + void CalibrationAuto(); + void DiodesOff(); + void DiodeOn(int numeroLed, int duree); + void EnregistreDonnee(int adresse, int valeur); + int LireDonnee(int adresse); + + + +void setup() //premiere fonction appelée. +{ + // initialize serial communication: + Serial.begin(9600); + Serial.println(10, DEC); + + + myservoRoueGauche.attach(pinRoueG,200,2400);//initialisation des servos //100,1500 || 220,2400 + myservoRoueDroite.attach(pinRoueD,220,2400); //80,1500 || 200,2400 + myservoRoueDroite.write(90);//90 + myservoRoueGauche.write(90);//90 + + + pinMode(pinTirr, INPUT);//la tirette est une entrée d'information + pinMode(pinCalClaire, INPUT); + pinMode(pinCalObs, INPUT); + pinMode(pinCalGD, INPUT); + pinMode(pinLed1,OUTPUT); + pinMode(pinLed2,OUTPUT); + pinMode(pinLed3,OUTPUT); + + pinMode(13, OUTPUT); + + + //Initialisation de la calibration capteurs couleurs à partir de l'EEPROM + seuilHautRattrapeeG=LireDonnee(0); + seuilHautRattrapageG=LireDonnee(1); + seuilHautRattrapeeD=LireDonnee(2); + seuilHautRattrapageD=LireDonnee(3); + + seuilBasRattrapeeG=LireDonnee(4); + seuilBasRattrapageG=LireDonnee(5); + seuilBasRattrapeeD=LireDonnee(6); + seuilBasRattrapageD=LireDonnee(7); + + seuilHautDifferenceRattrapageG=LireDonnee(8); + seuilHautDifferenceRattrapageD=LireDonnee(9); + + int tirretteAJamaisEtePresente=1; + + while(digitalRead(pinTirr)>0 || tirretteAJamaisEtePresente)//On attend que la tirette soit tirée pour débuter le match. Il faut aussi qu'elle ait été presente (sinon le robot partirai direct si allumé sans tirrette. + { + //Calibration : 2 modes : automatique (les 2 interrupteurs de calib' obscure (noir) et clair (bleu) sont activés) : on lance "CalibrationAuto". + //manuelle : seul un interrupteur de calib' (obs OU clair) est activé : on calibre le capteur gauche OU droit, selon l'interrupteur de choix. + + diode1.DiodeOff(); + diode2.DiodeOff(); + diode3.DiodeOff(); + DiodesOff();//On éteind les diodes qui doivent l'etre + + if(digitalRead(pinCalClaire)>0) + { + if(digitalRead(pinCalObs)>0)//calibration auto + { + CalibrationAuto(); + while((digitalRead(pinCalClaire)>0 || digitalRead(pinCalObs)>0) && (digitalRead(pinTirr)>0 || tirretteAJamaisEtePresente))// On attend que l'utilisateur mette le robot en attente d'une nouvelle calibration (les deux interrupteurs à l'arrt ; ou que le match commence. + { + //On regarde si la tirrette est là : + if (digitalRead(pinTirr)>0) + { + tirretteAJamaisEtePresente=0;//Alors on n'a plus qu'à attendre que la tirrette soit tirée. + } + delay(1); + } + } + + else//Calibration manuelle + { + if(digitalRead(pinCalGD)>0) + { + seuilBasRattrapeeG=paramSeuilBasRattrapee*analogRead(pinCCG); + EnregistreDonnee(4, seuilBasRattrapeeG); + + seuilBasRattrapageG=paramSeuilBasRattrapage*analogRead(pinCCG); + EnregistreDonnee(seuilBasRattrapageG, 5); + Serial.println(analogRead(pinCCG)); + Serial.println(9); + } + + + else + { + seuilBasRattrapeeD=paramSeuilBasRattrapee*analogRead(pinCCD); + EnregistreDonnee(6, seuilBasRattrapeeD); + + seuilBasRattrapageG=paramSeuilBasRattrapage*analogRead(pinCCD); + EnregistreDonnee(7, seuilBasRattrapageD); + Serial.println(analogRead(pinCCD)); + Serial.println(8); + } + } + } + else if(digitalRead(pinCalObs)>0) + { + if(digitalRead(pinCalClaire)>0)//calibration auto + { + CalibrationAuto(); + while((digitalRead(pinCalClaire)>0 || digitalRead(pinCalObs)>0) && digitalRead(pinTirr)>0)// On attend que l'utilisateur mette le robot en attente d'une nouvelle calibration (les deux interrupteurs à l'arrt ; ou que le match commence. + { + delay(1); + } + } + + else//Calibration manuelle + { + if(digitalRead(pinCalGD)>0) + { + seuilHautRattrapeeG=paramSeuilHautRattrapee*analogRead(pinCCG); + EnregistreDonnee(0, seuilHautRattrapeeG); + + seuilHautRattrapageG=paramSeuilHautRattrapage*analogRead(pinCCG); + EnregistreDonnee(1, seuilHautRattrapageG); + Serial.println(analogRead(pinCCG)); + Serial.println(7); + } + else + { + seuilHautRattrapeeD=paramSeuilHautRattrapee*analogRead(pinCCD); + EnregistreDonnee(2, seuilHautRattrapeeD); + + seuilHautRattrapageD=paramSeuilHautRattrapage*analogRead(pinCCD); + EnregistreDonnee(3, seuilHautRattrapageD); + Serial.println(analogRead(pinCCD)); + Serial.println(6); + } + } + } + + //On regarde si la tirrette est là : + if (digitalRead(pinTirr)>0) + { + tirretteAJamaisEtePresente=0;//Alors on n'a plus qu'à attendre que la tirrette soit tirée. + } + delay(1); + } + + tempDepartMatch=millis();//Référence pour s'arreter 90s plus tard. + + + + //Couleur de la zone de départ. + if(digitalRead(pinCalGD)==0) + { + couleurRobot = VIOLET; + + } + else + { + couleurRobot = ROUGE; + } +} + + + + +void loop()//Boucle principale, appelée dès que setup() est fini. +{ + + + + + /*DeplacementAuto("tout droit", 10000, 2); + Arret(); + delay(1000); + + DeplacementAuto("reculer", 2000, AUTO); + Arret(); + delay(10000);*/ + + + + //SEQUENCE INITIALE POUR REJOINDRE LA LIGNE. + + + //reculer + + DeplacementAuto("tout droit", 2500, AUTO); + Arret(); + delay(100); + + + if(couleurRobot==VIOLET) + { + //tourner à droite : + DeplacementAuto("droite", 900, AUTO); + + } + else + { + //tourner à gauche : + DeplacementAuto("gauche", 900, AUTO); + } + + + //ttdroit + DeplacementAuto("tout droit", 1000, AUTO); + + //VERRIFICATION QU'ON EST BIEN SUR LA LIGNE + if(couleurRobot==VIOLET) + { + int ligne=DeplacementAuto("gauche", 1200, 1); + + if(ligne==10 || ligne==0)//Si on n'a pas rattrappé la ligne en tournant à droite + { + ligne=DeplacementAuto("droite", 2400, 1); + + if(ligne==10 || ligne==0)//Si on n'a pas rattrappé la ligne en tournant à gauche + { + DeplacementAuto("gauche", 1250, 1);//On se recentre et on va tout droit... en esperant que ça passe... + } + else//Sinon, si on a bien rattrappé la ligne + { + DeplacementAuto("tout droit", 600, AUTO); + DeplacementAuto("gauche", 400, AUTO); + } + } + + + } + else + { + int ligne=DeplacementAuto("droite", 1200, 1); + + if(ligne==10 || ligne==0)//Si on n'a pas rattrappé la ligne en tournant à droite + { + ligne=DeplacementAuto("gauche", 2400, 1); + + if(ligne==10 || ligne==0)//Si on n'a pas rattrappé la ligne en tournant à gauche + { + DeplacementAuto("droite", 1250, 1);//On se recentre et on va tout droit... en esperant que ça passe... + } + } + else//Sinon, si on a bien rattrappé la ligne + { + + } + + } + Arret(); + delay(100); + + + + // SEQUENCE DE SUIVAGE DE LIGNE // + + // while(!bouteilleUnePoussee) + unsigned long tempsDepartSuivageLigne = millis(); + unsigned long tempsPasseArreteSuivreLigne = 0; + unsigned long debutArretSuivreLigne = 0; + unsigned long tempsMiniSuivageLigne = 8000;//Le temps minimal qu'il faut au robot pour suivre la ligne : permet de ne pas taper un robot par erreur + unsigned long tempsActuel = 0; + int dejaArreteSuiveurLigne = 0; + while(!bouteilleUneAPousser) + { + diode1.DiodeOff(); + diode2.DiodeOff(); + diode3.DiodeOff(); + DiodesOff();//On éteind les diodes qui doivent l'etre + tempsActuel=millis(); + if(tempsActueltempsMiniSuivageLigne && (detection.JePeuxAvancer(AVANCERBOUTEILLE)==2))//-tempsPasseArreteSuivreLigne + { + bouteilleUneAPousser=1; + } + else + { + + Arret(); + if(!dejaArreteSuiveurLigne)//si c'est le début de l'arret + { + dejaArreteSuiveurLigne=1; + debutArretSuivreLigne=tempsActuel; + } + + } + } + } + else//Si le match est fini + { + while(1) + { + Arret();//on arrete tout + delay(10); + } + } + + } + Arret(); + delay(1000); + DeplacementAuto("pousseBouteille", 1000, AUTO);//500 + Arret(); + delay(100); + DeplacementAuto("reculer", 1800, AUTO); + Arret(); + delay(500); + + + if(couleurRobot==ROUGE) + { +//tourner à droite : +DeplacementAuto("droite", 1100, AUTO); + + } + else + { + //tourner à gauche : + DeplacementAuto("gauche", 1100, AUTO); + + } + Arret(); + delay(500); + DeplacementAuto("reculer", 800, AUTO);//5000 + Arret(); + delay(500); + DeplacementAuto("reculer lent", 1000, AUTO);//5000 + + /*Arret(); + delay(500); + DeplacementAuto("tout droit", 500, AUTO);//5000 + + Arret(); + delay(500); + if(couleurRobot==ROUGE) + { + //tourner à droite : + DeplacementAuto("droite", 50, AUTO); + } + else + { + //tourner à gauche : + DeplacementAuto("gauche", 50, AUTO); + + }*/ + Arret(); + delay(500); + //DeplacementAuto("tout droit", 6000, AUTO);//5000 + DeplacementAuto("tout droit", 7000, AUTO); + Arret(); + delay(500); + DeplacementAuto("reculer", 500, AUTO);//5000 + + Arret(); + delay(500); + if(couleurRobot==VIOLET) + { +//tourner à droite : +DeplacementAuto("droite", 850, AUTO); + } + else + { + //tourner à gauche : + DeplacementAuto("gauche", 850, AUTO); + + } + + Arret(); + delay(300); + + + + //DeplacementAuto("tout droit", 2000, 2); + DeplacementAuto("tout droit", 500, AUTO); + Arret(); + delay(300); + + DeplacementAuto("pousseBouteille", 1000, AUTO); + Arret(); + delay(100); + //DeplacementAuto("reculer", 1000, AUTO); + Arret(); + + delay(1000000000); +} + + +//############VERSION 1################## +//marche bien, mais pas idéal : peu rapide et risque de perdre la ligne si le robot a un angle trop important avec elle. +void SuivreLigne()//Permet de se déplacer en suivant la ligne. +{ + int valCCG = analogRead(pinCCG);//valeur du CapteurCouleurGauche + int valCCD = analogRead(pinCCD); + + + + //if(valCCG > seuilHautRattrapageG && valCCD < seuilBasRattrapageG)//Si la ligne est entièrement sous le capteur gauche, et pas du tout sur le capteur droit + if(valCCG > seuilHautDifferenceRattrapageG)//Si la ligne est entièrement sous le capteur gauche, et pas du tout sur le capteur droit + { + myservoRoueGauche.write(90);//90 + myservoRoueDroite.write(30);//82 + + //delay(500); + + //Rattrapage("gauche");//on lance la procédure de rattrapage. + + + } + //else if(valCCD > seuilHautRattrapageD && valCCG < seuilBasRattrapageD)//Idem + else if(valCCD > seuilHautDifferenceRattrapageD)//Idem + { + myservoRoueGauche.write(150);//98 + myservoRoueDroite.write(90);//90 + //delay(500); + //Rattrapage("droite"); + + } + else//Sinon, tout va bien + { + //Serial.println(00000); + + myservoRoueGauche.write(95); + myservoRoueDroite.write(83); + + //myservoRoueGauche.write(valMoyenneG-(offsetG+valCCG)*paramG+(offsetD+valCCD)*paramD);//On fait tourner les servos, en fonction des valeurs renvoyées par les capteurs. + //myservoRoueDroite.write(valMoyenneD-(offsetD+valCCD)*paramD+(offsetG+valCCG)*paramG); + + } +} + + +int DeplacementAuto(char* dir, unsigned long duree, int modeRattrappageLigne)//Permet de se déplacer sans la ligne. v2 +{ + unsigned long tempsDebut=millis(); + + unsigned long currentTime=millis(); + + unsigned long delaiArret = 0;//Sile robot doit s'arreter (par exemple s'il detecte un robot adverse), il faut qu'il finisse son mouvement plus tard que prevu. + + unsigned long debutArret = 0; + + int dejaArrete = 0; + + int valeurRetour = 0; + + while((currentTime seuilHautDifferenceRattrapageG)//valeur du CapteurCouleurGauche + { + Arret(); + delay(100); + DeplacementAuto("gauche", 100, AUTO); + Arret(); + delay(100); + //delay(500); + valeurRetour++; + + //diode3.DiodeOn(2);//Allume la diode pendant 2 cycles. + /* digitalWrite(pinLed2,HIGH); + delay(1000); + digitalWrite(pinLed2,LOW);*/ + } + if(analogRead(pinCCD) > seuilHautDifferenceRattrapageD)//valeur du CapteurCouleurGauche + { + Arret(); + delay(100); + DeplacementAuto("droite", 100, AUTO); + Arret(); + delay(100); + //delay(500); + valeurRetour+=2; + + //diode3.DiodeOn(2);//Allume la diode pendant 2 cycles. + /* digitalWrite(pinLed1,HIGH); + delay(1000); + digitalWrite(pinLed1,LOW);*/ + } + + if(valeurRetour>0) + { + return valeurRetour; + } + } + /*else if(modeRattrappageLigne==2)//Si on veut se mettre en place pour pousser une 2° bouteille + { + detection.JePeuxAvancer(AVANCERBOUTEILLE)==2; + return 20; + }*/ + + //Serial.println( dejaArrete); + if((currentTime+tempsMaxCapteurs>tempsDebut+duree+delaiArret) && !dejaArrete)//Si le mouvement est en cours et se fini avant qu'on ait le temps de faire une boucle supplémentaire, on attend la fin du mouvement. + { + if(currentTime0))) + { + + + if(dejaArrete)//Si on sort d'une période d'arret + { + dejaArrete=0; + delaiArret+=millis()-debutArret; + } + if(dir=="reculer") + { + myservoRoueGauche.write(ReculerRoueG); + myservoRoueDroite.write(ReculerRoueD); + } + else if(dir=="tout droit") + { + + myservoRoueGauche.write(TTDroitRoueG); + myservoRoueDroite.write(TTDroitRoueD); + } + + else if(dir=="droite") + { + myservoRoueGauche.write(TournerDroiteRoueG); + myservoRoueDroite.write(TournerDroiteRoueD); + } + else if(dir=="gauche") + { + myservoRoueGauche.write(TournerGaucheRoueG); + myservoRoueDroite.write(TournerGaucheRoueD); + } + else if(dir=="pousseBouteille") + { + myservoRoueGauche.write(TTDroitRoueG); + myservoRoueDroite.write(TTDroitRoueD); + } + else if(dir=="reculer lent") + { + myservoRoueGauche.write(ReculerLentRoueG); + myservoRoueDroite.write(ReculerLentRoueD); + } + + } + + else//Sinon, s'il y a un obstacle + { + Arret(); + if(!dejaArrete)//si c'est le début de l'arret + { + dejaArrete=1; + debutArret=millis(); + } + delay(1); + } + + } + + currentTime=millis();//Le temps actuel pour le prochain tour de boucle. + } + + return 0; +} + + + +void Arret()//Arrete les moteurs. +{ + myservoRoueGauche.write(paramArretRoueG); + myservoRoueDroite.write(paramArretRoueD); +} + + + +void CalibrationAuto()//Permet d'auto-calibrer le robot. Il faut le placer sur une ligne noire et enclencher les deux interrupteurs "cal obsc" et "cal clair". +{ + Arret(); + delay(500); + + //On est sensé etre sur la ligne noire, donc on calibre. + hautGauche=analogRead(pinCCG); + hautDroite=analogRead(pinCCD); + + seuilHautRattrapeeG=paramSeuilHautRattrapee*analogRead(pinCCG); + EnregistreDonnee(0, seuilHautRattrapeeG); + + seuilHautRattrapageG=paramSeuilHautRattrapage*analogRead(pinCCG); + EnregistreDonnee(1, seuilHautRattrapageG); + + seuilHautRattrapeeD=paramSeuilHautRattrapee*analogRead(pinCCD); + EnregistreDonnee(2, seuilHautRattrapeeD); + + seuilHautRattrapageD=paramSeuilHautRattrapage*analogRead(pinCCD); + EnregistreDonnee(3, seuilHautRattrapageD); + delay(300); + //On avance, pour arriver sur du blanc, puis on calibre. + myservoRoueGauche.write(95); + myservoRoueDroite.write(85); + delay(500); + Arret(); + delay(1000); + + basGauche=analogRead(pinCCG); + basDroite=analogRead(pinCCD); + + seuilBasRattrapeeG=paramSeuilBasRattrapee*analogRead(pinCCG); + EnregistreDonnee(4, seuilBasRattrapeeG); + + seuilBasRattrapageG=paramSeuilBasRattrapage*analogRead(pinCCG); + EnregistreDonnee(5, seuilBasRattrapageG); + + seuilBasRattrapeeD=paramSeuilBasRattrapee*analogRead(pinCCD); + EnregistreDonnee(6, seuilBasRattrapeeD); + + seuilBasRattrapageD=paramSeuilBasRattrapage*analogRead(pinCCD); + EnregistreDonnee(7, seuilBasRattrapageD); + + + + seuilHautDifferenceRattrapageG=hautGauche-(hautGauche-basGauche)/paramDiff; + EnregistreDonnee(8, seuilHautDifferenceRattrapageG); + + seuilHautDifferenceRattrapageD=hautDroite-(hautDroite-basDroite)/paramDiff; + EnregistreDonnee(9, seuilHautDifferenceRattrapageD); + + //On enregistre dans la mémoire eeprom, pour pouvoir tester sans avoir à re-qualibrer. + //EEPROM.write(0,seuilHautRattrapeeG); +} + +//On éteind les diodes qui doivent l'etre +//permet d'avoir des diodes allumées pendant des cycles entiers, pour qu'elles soient suffisament lumineuses. +void DiodesOff() +{ + if(cycleAllumeDiode1>1) + { + cycleAllumeDiode1--; + } + else + { + if(cycleAllumeDiode1==1) + { + digitalWrite(pinLed1,LOW); + cycleAllumeDiode1--; + } + } + if(cycleAllumeDiode2>0) + { + cycleAllumeDiode2--; + } + else + { + if(cycleAllumeDiode2==1) + { + digitalWrite(pinLed2,LOW); + cycleAllumeDiode2--; + } + } + if(cycleAllumeDiode3>0) + { + cycleAllumeDiode3--; + } + else + { + if(cycleAllumeDiode3==1) + { + digitalWrite(pinLed3,LOW); + cycleAllumeDiode3--; + } + } + +} + +void DiodeOn(int numeroLed, int duree) +{ + if(numeroLed==pinLed1) + { + cycleAllumeDiode1=duree; + digitalWrite(pinLed1,HIGH); + } + if(numeroLed==pinLed2) + { + cycleAllumeDiode2=duree; + digitalWrite(pinLed2,HIGH); + } + if(numeroLed==pinLed3) + { + cycleAllumeDiode3=duree; + digitalWrite(pinLed3,HIGH); + } +} + +void EnregistreDonnee(int adresse, int valeur)//Enregistre un nombre entre 0 et 1023 dans l'EEPROM +{ + if(valeur>=3*256) + { + EEPROM.write(adresse*4+3,valeur-3*256);//Reste bien compris entre 0 et 255 + EEPROM.write(adresse*4+2,255); + EEPROM.write(adresse*4+1,255); + EEPROM.write(adresse*4+0,255); + } + else + { + EEPROM.write(adresse*4+3,0); + if(valeur>=2*256) + { + EEPROM.write(adresse*4+2,valeur-2*256);//Reste bien compris entre 0 et 255 + EEPROM.write(adresse*4+1,255); + EEPROM.write(adresse*4+0,255); + } + else + { + EEPROM.write(adresse*4+2,0); + if(valeur>=1*256) + { + EEPROM.write(adresse*4+1,valeur-1*256);//Reste bien compris entre 0 et 255 + EEPROM.write(adresse*4+0,255); + } + else + { + EEPROM.write(adresse*4+1,0); + EEPROM.write(adresse*4+0,valeur-0*256);//Reste bien compris entre 0 et 255 + } + } + } +} + +int LireDonnee(int adresse)//Lit un nombre entre 0 et 1023 dans l'EEPROM +{ + int valeur = 0; + int lu = 0; + valeur += EEPROM.read(adresse*4+0); + + lu=EEPROM.read(adresse*4+1); + if(lu) + { + valeur+=lu+1;//EEPROM.read va de 0 à 255 + lu=EEPROM.read(adresse*4+2); + if(lu) + { + valeur+=lu+1;//EEPROM.read va de 0 à 255 + lu=EEPROM.read(adresse*4+3); + if(lu) + { + valeur+=lu+1;//EEPROM.read va de 0 à 255 + } + } + } + return valeur; +} + diff --git a/petit_robot/Sharp.cpp b/petit_robot/Sharp.cpp new file mode 100644 index 00000000..9f65dc4e --- /dev/null +++ b/petit_robot/Sharp.cpp @@ -0,0 +1,41 @@ +#include "Sharp.h" +#include "Arduino.h" + + +Sharp::Sharp(): Capteur(0) +{ + this->seuilDetection=200; +} + +Sharp::Sharp(int capteurPin, int seuil) : Capteur(capteurPin, valeur), seuilDetection(seuil) +{ + //this->seuilDetection=200; + //valeur=0; + +} + +void Sharp::Update() +{ + valeur = analogRead(capteurPin); + //Serial.println( capteurPin); + //Serial.println( analogRead(capteurPin)); + //valeur=0; +} + +int Sharp::VoitQQch() +{ + Update(); + //if(valeur>seuilDetection) + + if(valeur>seuilDetection) + { + + //delay(100); + return 1; + } + else + { + //Serial.println(0); + return 0; + } +} diff --git a/petit_robot/Sharp.h b/petit_robot/Sharp.h new file mode 100644 index 00000000..0b911768 --- /dev/null +++ b/petit_robot/Sharp.h @@ -0,0 +1,23 @@ +#ifndef DEF_SHARP // Si la constante n'a pas été définie` le fichier n'a jamais été inclus +#define DEF_SHARP // On définit la constante pour que la prochaine fois le fichier ne soit plus inclus + + +#include "Capteur.h" + +class Sharp : public Capteur//Un sharp est un capteur : héritage ! +{ + public: + + Sharp(); + Sharp(int capteurPin, int seuil); + void Update();//Met à jour la valeur du capteur. + int VoitQQch();//Renvoit 1 si le capteur voie qqch (d'après seuil), 0 sinon. + + + private: + + int seuilDetection; + +}; + +#endif diff --git a/petit_robot/Ultrason.cpp b/petit_robot/Ultrason.cpp new file mode 100644 index 00000000..604637e7 --- /dev/null +++ b/petit_robot/Ultrason.cpp @@ -0,0 +1,70 @@ +#include "Ultrason.h" +#include "Arduino.h" +#include "Capteur.h" + +#define DELAI_ATTENTE_MAX 10000//On max, on attent 10ms que le son revienne. +#define DISTANCE_MAX_DETECTION 25//S'il y a qqch à moins de 25cm, on s'arrete. +#define DIST_MIN_DETECTION 1//si le capteur renvoit 0, c'est soit que l'objet est très loin, soit qu'il y a un pb. + +Ultrason::Ultrason() : Capteur(0)//Je ne vois pas trop comment faire sans mettre un constructeur bidon... +{ + +} + + +Ultrason::Ultrason(int capteurPin) : Capteur(capteurPin) +{ + +} + +int Ultrason::VoitQQch() +{ + Update(); + if(distance < DISTANCE_MAX_DETECTION) + { + return 1; + } + else + { + return 0; + } +} + +//int Ultrason::Distance()//Renvoit la distance du plus proche objet du capteur +void Ultrason::Update() +{ + + + // establish variables for duration of the ping, + // and the distance result in inches and centimeters: + long duration, cm; + + // The PING))) is triggered by a HIGH pulse of 2 or more microseconds. + // Give a short LOW pulse beforehand to ensure a clean HIGH pulse: + pinMode(capteurPin, OUTPUT); + digitalWrite(capteurPin, LOW); + delayMicroseconds(2); + digitalWrite(capteurPin, HIGH); + delayMicroseconds(5); + digitalWrite(capteurPin, LOW); + + // The same pin is used to read the signal from the PING))): a HIGH + // pulse whose duration is the time (in microseconds) from the sending + // of the ping to the reception of its echo off of an object. + pinMode(capteurPin, INPUT); + duration = pulseIn(capteurPin, HIGH,10000);//On n'attend pas plus de 10 millisecondes le signal de retour. + + // convert the time into a distance + cm = duration / 29 / 2; + // The speed of sound is 340 m/s or 29 microseconds per centimeter. + // The ping travels out and back, so to find the distance of the + // object we take half of the distance travelled. + + if(cm