Zumo32U4 Combat

/* Exemple Zumo32U4 sumo adapté :
   - Surface de combat : sol gris
   - Bordure / mur : tape noir au sol
 
   Le robot :
   - Utilise les capteurs de ligne pour détecter la bordure noire (mur) et reculer.
   - Utilise les capteurs de proximité pour chercher l’adversaire et foncer dessus.
 
   Pour que le code fonctionne, les jumpers de la front sensor array
   doivent connecter :
     - pin 4 -> RGT
     - pin 20 -> LFT
 
   Testé avec un Zumo 32U4 et moteurs 75:1 HP.
*/
 
#include <Wire.h>
#include <Zumo32U4.h>
 
// Si tu as l’ancien écran noir/vert, utilise Zumo32U4LCD.
// Ici : OLED.
Zumo32U4OLED display;
 
Zumo32U4ButtonA buttonA;
Zumo32U4Buzzer buzzer;
Zumo32U4Motors motors;
Zumo32U4LineSensors lineSensors;
Zumo32U4ProximitySensors proxSensors;
 
unsigned int lineSensorValues[3];
 
// *** IMPORTANT ***
// On a : sol gris au centre, bordure en tape noir.
// Quand la lecture sur un capteur de ligne VA AU-DESSUS de ce seuil,
// on considère qu’il détecte le MUR (tape noir) à la bordure.
// --> Ajuste lineSensorThreshold après avoir mesuré:
//     - valeur au centre (gris)
//     - valeur sur le tape noir
//     Mets un seuil entre les 2 et vérifie que la condition (>) marche.
const uint16_t lineSensorThreshold = 1000;
 
// Vitesse en marche arrière
const uint16_t reverseSpeed = 200;
 
// Vitesse pour tourner sur place
const uint16_t turnSpeed = 200;
 
// Vitesse en marche avant (mode normal)
const uint16_t forwardSpeed = 200;
 
// Vitesses pour “dévier” vers la gauche ou la droite en cherchant l’ennemi
const uint16_t veerSpeedLow  = 0;
const uint16_t veerSpeedHigh = 250;
 
// Vitesse de “ramming” (quand on pense pousser l’adversaire)
const uint16_t rammingSpeed = 400;
 
// Temps à passer en marche arrière après avoir détecté la bordure (ms)
const uint16_t reverseTime = 200;
 
// Temps min/max pour scanner en tournant (ms)
const uint16_t scanTimeMin = 200;
const uint16_t scanTimeMax = 2100;
 
// Temps d’attente après appui bouton A avant de bouger (ms) (règle sumo)
const uint16_t waitTime = 5000;
 
// Temps à avancer tout droit avant de décider qu’on est en “stalemate” (ms)
const uint16_t stalemateTime = 4000;
 
// États principaux de la machine à états
enum State
{
  StatePausing,
  StateWaiting,
  StateScanning,
  StateDriving,
  StateBacking,
};
 
State state = StatePausing;
 
enum Direction
{
  DirectionLeft,
  DirectionRight,
};
 
// Direction de scan la prochaine fois qu’on cherchera un adversaire
Direction scanDir = DirectionLeft;
 
// Temps (ms) d’entrée dans l’état courant
uint16_t stateStartTime;
 
// Temps (ms) de dernier refresh de l’affichage
uint16_t displayTime;
 
// Vrai quand on vient de changer d’état
bool justChangedState;
 
// Vrai quand l’écran vient d’être effacé
bool displayCleared;
 
void setup()
{
  // Décommente si un moteur tourne à l’envers :
  // motors.flipLeftMotor(true);
  // motors.flipRightMotor(true);
 
  // On utilise les 3 capteurs de ligne (gauche, milieu, droite)
  lineSensors.initThreeSensors();
 
  // On utilise aussi les 3 capteurs de proximité (gauche, front, droite)
  proxSensors.initThreeSensors();
 
  changeState(StatePausing);
}
 
void loop()
{
  bool buttonPress = buttonA.getSingleDebouncedPress();
 
  if (state == StatePausing)
  {
    // État de pause : on affiche la tension batterie
    // et on attend que l’utilisateur appuie sur A.
 
    motors.setSpeeds(0, 0);
 
    if (justChangedState)
    {
      justChangedState = false;
      display.print(F("Press A"));
    }
 
    if (displayIsStale(100))
    {
      displayUpdated();
      display.gotoXY(0, 1);
      display.print(readBatteryMillivolts());
    }
 
    if (buttonPress)
    {
      // Passage à l’état d’attente avant départ
      changeState(StateWaiting);
    }
  }
  else if (buttonPress)
  {
    // Appui sur A pendant que le robot tourne -> on met en pause.
    changeState(StatePausing);
  }
  else if (state == StateWaiting)
  {
    // On attend waitTime ms avant de commencer à bouger.
 
    motors.setSpeeds(0, 0);
 
    uint16_t time = timeInThisState();
 
    if (time < waitTime)
    {
      // Affiche le temps restant (en secondes, avec 1 décimale)
      uint16_t timeLeft = waitTime - time;
      display.gotoXY(0, 0);
      display.print(timeLeft / 1000 % 10);
      display.print('.');
      display.print(timeLeft / 100 % 10);
    }
    else
    {
      // Assez attendu, on commence le scan de l’adversaire
      changeState(StateScanning);
    }
  }
  else if (state == StateBacking)
  {
    // Marche arrière après avoir détecté la bordure.
 
    if (justChangedState)
    {
      justChangedState = false;
      display.print(F("back"));
    }
 
    motors.setSpeeds(-reverseSpeed, -reverseSpeed);
 
    // Après reverseTime ms, on repasse en scan
    if (timeInThisState() >= reverseTime)
    {
      changeState(StateScanning);
    }
  }
  else if (state == StateScanning)
  {
    // Robot tourne sur place et cherche l’adversaire.
 
    if (justChangedState)
    {
      justChangedState = false;
      display.print(F("scan"));
    }
 
    if (scanDir == DirectionRight)
    {
      motors.setSpeeds(turnSpeed, -turnSpeed);
    }
    else
    {
      motors.setSpeeds(-turnSpeed, turnSpeed);
    }
 
    uint16_t time = timeInThisState();
 
    if (time > scanTimeMax)
    {
      // Rien vu depuis longtemps -> on avance
      changeState(StateDriving);
    }
    else if (time > scanTimeMin)
    {
      // On regarde les capteurs de proximité frontaux
      proxSensors.read();
      if (proxSensors.countsFrontWithLeftLeds()  >= 2
       || proxSensors.countsFrontWithRightLeds() >= 2)
      {
        // Adversaire détecté devant -> on avance
        changeState(StateDriving);
      }
    }
  }
  else if (state == StateDriving)
  {
    // On avance, on surveille :
    // - la bordure (mur noir),
    // - la présence de l’adversaire devant / sur les côtés.
 
    if (justChangedState)
    {
      justChangedState = false;
      display.print(F("drive"));
    }
 
    // *** DÉTECTION BORDURE ***
    // On lit les 3 capteurs de ligne.
    lineSensors.read(lineSensorValues);
 
    // Avec sol gris + tape noir :
    // -> on considère qu’on voit le MUR si la valeur est AU-DESSUS du seuil.
    if (lineSensorValues[0] > lineSensorThreshold)
    {
      // Bordure détectée côté gauche -> on reculera puis on tournera vers la droite
      scanDir = DirectionRight;
      changeState(StateBacking);
    }
    if (lineSensorValues[2] > lineSensorThreshold)
    {
      // Bordure détectée côté droit -> on reculera puis on tournera vers la gauche
      scanDir = DirectionLeft;
      changeState(StateBacking);
    }
 
    // *** DÉTECTION ADVERSAIRE PAR PROXIMITÉ ***
    proxSensors.read();
    uint8_t sum  = proxSensors.countsFrontWithRightLeds()
                 + proxSensors.countsFrontWithLeftLeds();
    int8_t diff  = proxSensors.countsFrontWithRightLeds()
                 - proxSensors.countsFrontWithLeftLeds();
 
    if (sum >= 4 || timeInThisState() > stalemateTime)
    {
      // Forte détection frontale OU on avance depuis longtemps -> on fonce (ramming)
      motors.setSpeeds(rammingSpeed, rammingSpeed);
 
      // LED rouge allumée quand on “ram”
      ledRed(1);
    }
    else if (sum == 0)
    {
      // Rien devant, on continue tout droit en mode normal
      motors.setSpeeds(forwardSpeed, forwardSpeed);
 
      // Mais on surveille les côtés pour repasser en scan si on voit qqch
      if (proxSensors.countsLeftWithLeftLeds() >= 2)
      {
        // Objet sur la gauche
        scanDir = DirectionLeft;
        changeState(StateScanning);
      }
 
      if (proxSensors.countsRightWithRightLeds() >= 2)
      {
        // Objet sur la droite
        scanDir = DirectionRight;
        changeState(StateScanning);
      }
 
      ledRed(0);
    }
    else
    {
      // On voit quelque chose devant mais pas super fort :
      // on “pivote” légèrement pour viser l’adversaire.
 
      if (diff >= 1)
      {
        // Lecture droite plus forte -> on vire à droite
        motors.setSpeeds(veerSpeedHigh, veerSpeedLow);
      }
      else if (diff <= -1)
      {
        // Lecture gauche plus forte -> on vire à gauche
        motors.setSpeeds(veerSpeedLow, veerSpeedHigh);
      }
      else
      {
        // Égal -> tout droit
        motors.setSpeeds(forwardSpeed, forwardSpeed);
      }
      ledRed(0);
    }
  }
}
 
// Temps passé dans l’état courant (ms).
// Overflow après ~65s, mais ce n’est pas grave ici.
uint16_t timeInThisState()
{
  return (uint16_t)(millis() - stateStartTime);
}
 
// Change d’état, reset les LEDs et efface l’écran.
void changeState(uint8_t newState)
{
  state = (State)newState;
  justChangedState = true;
  stateStartTime = millis();
  ledRed(0);
  ledYellow(0);
  ledGreen(0);
  display.clear();
  displayCleared = true;
}
 
// True si l’affichage est “vieux” de plus que staleTime (ms)
// ou vient d’être clear.
bool displayIsStale(uint16_t staleTime)
{
  return displayCleared || (millis() - displayTime) > staleTime;
}
 
// À appeler à chaque fois qu’on met à jour l’écran.
void displayUpdated()
{
  displayTime = millis();
  displayCleared = false;
}