Automower-fans

Code : Tout sélectionner

#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_ADXL345_U.h>
#include <DFRobot_QMC5883.h>

Adafruit_ADXL345_Unified accel = Adafruit_ADXL345_Unified(12345);
DFRobot_QMC5883 compass(&Wire, 0xD);

// DECLARATION DES BROCHES
#define MOTEUR_ROUE_GAUCHE 6                 // Broche pour le moteur de la roue gauche
#define MOTEUR_ROUE_DROITE 7                 // Broche pour le moteur de la roue droite
#define DIR_MOTEUR_ROUE_GAUCHE 5             // Broche pour la direction du moteur de la roue gauche
#define DIR_MOTEUR_ROUE_DROITE 4             // Broche pour la direction du moteur de la roue droite
#define CAPTEUR_FIL_PERPH_AVANT A5           // Broche pour le capteur de fil périphérique avant
#define CAPTEUR_FIL_PERPH_LATERAL_GAUCHE A1  // Broche pour le capteur de fil périphérique latéral gauche
#define CAPTEUR_FIL_PERPH_LATERAL_DROIT A0   // Broche pour le capteur de fil périphérique latéral droit

// Seuils des capteurs
#define VAL_DECL_CAPTEUR 50      // Seuil de détection du capteur avant
#define VAL_DECL_CAPTEUR_LAT 50  // Seuil de détection des capteurs latéraux
#define AMPLITUDE_CHOC_AVANCE 4
#define AMPLITUDE_CHOC_LATERAL 5
#define CORRECTEUR_CAP 0.8
#define KP_SUIVI_CAP 4

// VITESSES
#define VITESSE_DEPLACEMENT_TONTE 180  // Vitesse de déplacement rapide
#define VITESSE_APPROCHE 60            // Vitesse d'approche

// VARIABLES
int VAL_CAP_AVANT = 0;  // Valeur du capteur avant
int VAL_CAP_AVANT_ANCIEN;
int VAL_CAP_LATERAL_GAUCHE = 0;  // Valeur du capteur latéral gauche
int VAL_CAP_LATERAL_GAUCHE_ANCIEN;
int VAL_CAP_LATERAL_DROIT = 0;  // Valeur du capteur latéral droit
int VAL_CAP_LATERAL_DROIT_ANCIEN;

int CAP_ACTUEL;
int CAP_CIBLE = 90;

void setup() {
  Serial.begin(9600);
  // Configuration des broches en sortie
  pinMode(MOTEUR_ROUE_GAUCHE, OUTPUT);
  pinMode(MOTEUR_ROUE_DROITE, OUTPUT);
  pinMode(DIR_MOTEUR_ROUE_GAUCHE, OUTPUT);
  pinMode(DIR_MOTEUR_ROUE_DROITE, OUTPUT);

  if (!accel.begin()) {
    while (1)
      Serial.println("Ooops, no ADXL345 detected ... Check your wiring!");
  }
  if (!compass.begin()) {
    while (1)
      Serial.println("Could not find a valid QMC5883 sensor, check wiring!");
  } else if (compass.isQMC()) {
    Serial.println("Initialize QMC5883");
  }

  // Démarrage de la communication série
}

void loop() {
  float declinationAngle = (4.0 + (26.0 / 60.0)) / (180 / PI);
  compass.setDeclinationAngle(declinationAngle);
  sVector_t mag = compass.readRaw();
  compass.getHeadingDegrees();
  CAP_ACTUEL = mag.HeadingDegress*CORRECTEUR_CAP;

  // Lecture des capteurs
  VAL_CAP_AVANT = (analogRead(CAPTEUR_FIL_PERPH_AVANT) * 0.6) + (VAL_CAP_AVANT_ANCIEN * 0.4);
  VAL_CAP_LATERAL_GAUCHE = (analogRead(CAPTEUR_FIL_PERPH_LATERAL_GAUCHE) * 0.6) + (VAL_CAP_LATERAL_GAUCHE_ANCIEN * 0.4);
  VAL_CAP_LATERAL_DROIT = (analogRead(CAPTEUR_FIL_PERPH_LATERAL_DROIT) * 0.6) + (VAL_CAP_LATERAL_DROIT_ANCIEN * 0.4);

  // Lecture de l'accéléromètre
  sensors_event_t event;
  accel.getEvent(&event);

  // Vérification de la détection de choc
  if (event.acceleration.x > AMPLITUDE_CHOC_AVANCE || event.acceleration.y > AMPLITUDE_CHOC_LATERAL) {
    // Si un choc est détecté, arrêter les moteurs
    ARRET();
    while (1) { Serial.println("Choc détecté. Arrêt du robot."); }
  }

  // Affichage des valeurs des capteurs dans le moniteur série

  Serial.print("CAPT_BORD: ");
  Serial.print(VAL_CAP_LATERAL_GAUCHE);
  Serial.print(" - ");
  Serial.print(VAL_CAP_AVANT);
  Serial.print(" - ");
  Serial.print(VAL_CAP_LATERAL_DROIT);
  Serial.print(" / CAP_CIBLE: ");
  Serial.print(CAP_CIBLE);
  Serial.print(" - CAP_ACTUEL: ");
  Serial.println(CAP_ACTUEL);


  // Logique de déplacement en fonction des valeurs des capteurs
  if ((VAL_CAP_AVANT > VAL_DECL_CAPTEUR) || (VAL_CAP_LATERAL_GAUCHE > VAL_DECL_CAPTEUR_LAT && VAL_CAP_LATERAL_DROIT > VAL_DECL_CAPTEUR_LAT)) {
    Serial.println("FIL PERIPHERIQUE DETECTE - DEMI TOUR");
    // Si le capteur avant détecte le fil périphérique, arrêter et reculer
    ARRET();
    delay(800);  // Attendre un moment
    DEMI_TOUR_DROITE(VITESSE_APPROCHE, 4000);
    CAP_CIBLE = (CAP_CIBLE + 180) % 360;
    delay(800);  // Réajuster le délai si nécessaire
  } else if (VAL_CAP_LATERAL_GAUCHE > VAL_DECL_CAPTEUR_LAT) {
    Serial.println("FIL PERIPHERIQUE DETECTE - ORIENTATION");  // Si le capteur latéral gauche détecte le fil périphérique, tourner à droite
    VIRAGE_DROITE(VITESSE_APPROCHE, 200);                      // Réajuster la durée de virage si nécessaire
  } else if (VAL_CAP_LATERAL_DROIT > VAL_DECL_CAPTEUR_LAT) {
    // Si le capteur latéral droit détecte le fil périphérique, tourner à gauche
    VIRAGE_GAUCHE(VITESSE_APPROCHE, 200);  // Réajuster la durée de virage si nécessaire
    Serial.println("FIL PERIPHERIQUE DETECTE - ORIENTATION");
  } else {
    // Si aucun capteur ne détecte le fil périphérique, avancer
    AVANCER_CAP(CAP_CIBLE, VITESSE_DEPLACEMENT_TONTE);
  }
  VAL_CAP_AVANT_ANCIEN = VAL_CAP_AVANT;
  VAL_CAP_LATERAL_GAUCHE_ANCIEN = VAL_CAP_LATERAL_GAUCHE;
  VAL_CAP_LATERAL_DROIT_ANCIEN = VAL_CAP_LATERAL_DROIT;
}

// Fonctions de contrôle des mouvements

void AVANCE(int vitesse) {
  digitalWrite(DIR_MOTEUR_ROUE_GAUCHE, LOW);
  digitalWrite(DIR_MOTEUR_ROUE_DROITE, HIGH);
  analogWrite(MOTEUR_ROUE_GAUCHE, vitesse);
  analogWrite(MOTEUR_ROUE_DROITE, vitesse);
}

void AVANCER_CAP(int cap_cible, int vitesse) {
  float erreur_cap;
  const float tolerance_cap = 1;  // Tolérance d'erreur de cap

  // Calcul de l'erreur entre le cap actuel et le cap cible
  erreur_cap = cap_cible - CAP_ACTUEL;

  // Correction de l'erreur si elle dépasse 180 degrés
  if (erreur_cap > 180) {
    erreur_cap -= 360;
  } else if (erreur_cap < -180) {
    erreur_cap += 360;
  }

  // Si l'erreur est supérieure à la tolérance, corriger le cap en tournant
  if (abs(erreur_cap) > tolerance_cap) {
    if (erreur_cap > 0) {
      int correction = abs(erreur_cap)*KP_SUIVI_CAP;
      VIRAGE_FAIBLE_GAUCHE(vitesse, 100, correction);
    } else {
      int correction = abs(erreur_cap)*KP_SUIVI_CAP;
      VIRAGE_FAIBLE_DROITE(vitesse, 100, correction);
    }
  } else {
    // Si l'erreur est dans la tolérance, avancer en ligne droite
    AVANCE(vitesse);
  }
}

void ARRET() {
  analogWrite(MOTEUR_ROUE_GAUCHE, 0);
  analogWrite(MOTEUR_ROUE_DROITE, 0);
}

void VIRAGE_DROITE(int vitesse, int time) {
  digitalWrite(DIR_MOTEUR_ROUE_GAUCHE, LOW);
  digitalWrite(DIR_MOTEUR_ROUE_DROITE, LOW);
  analogWrite(MOTEUR_ROUE_GAUCHE, vitesse);
  analogWrite(MOTEUR_ROUE_DROITE, vitesse);
  delay(time);
  ARRET();
}

void VIRAGE_FAIBLE_DROITE(int vitesse, int time, int force) {
  digitalWrite(DIR_MOTEUR_ROUE_GAUCHE, LOW);
  digitalWrite(DIR_MOTEUR_ROUE_DROITE, HIGH);
  analogWrite(MOTEUR_ROUE_GAUCHE, vitesse - force);
  analogWrite(MOTEUR_ROUE_DROITE, vitesse);
  delay(time);
  ARRET();
}

void VIRAGE_GAUCHE(int vitesse, int time) {
  digitalWrite(DIR_MOTEUR_ROUE_GAUCHE, HIGH);
  digitalWrite(DIR_MOTEUR_ROUE_DROITE, HIGH);
  analogWrite(MOTEUR_ROUE_GAUCHE, vitesse);
  analogWrite(MOTEUR_ROUE_DROITE, vitesse);
  delay(time);
  ARRET();
}

void VIRAGE_FAIBLE_GAUCHE(int vitesse, int time, int force) {
  digitalWrite(DIR_MOTEUR_ROUE_GAUCHE, LOW);
  digitalWrite(DIR_MOTEUR_ROUE_DROITE, HIGH);
  analogWrite(MOTEUR_ROUE_GAUCHE, vitesse);
  analogWrite(MOTEUR_ROUE_DROITE, vitesse - force);
  delay(time);
  ARRET();
}

void DEMI_TOUR_DROITE(int vitesse, int time) {
  digitalWrite(DIR_MOTEUR_ROUE_GAUCHE, LOW);
  digitalWrite(DIR_MOTEUR_ROUE_DROITE, LOW);
  analogWrite(MOTEUR_ROUE_GAUCHE, vitesse);
  analogWrite(MOTEUR_ROUE_DROITE, vitesse);
  delay(time);
}