Zumo32U4 Combat
Aller à la navigation
Aller à la recherche
/* 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;
}