Castel'Lab le Fablab MJC de Château-Renault

Outils pour utilisateurs

Outils du site

Piste : • labyrinthe • inkscape
vittascience:labyrinthe

Différences

Ci-dessous, les différences entre deux révisions de la page.

Lien vers cette vue comparative

Les deux révisions précédentesRévision précédente
Prochaine révision		Révision précédente
vittascience:labyrinthe [2026/03/06 05:14] – adminvittascience:labyrinthe [2026/03/06 16:23] (Version actuelle) – [algorythme essai] admin
Ligne 52: Ligne 52:
        *14 Arrêt du robot lorsque la sortie du labyrinthe est détectée.        *14 Arrêt du robot lorsque la sortie du labyrinthe est détectée.
 </code> </code>
 +
 +==== algorythme essai ====
 +
 +  * 1- régler vitesse des moteurs entre 20% et 50%
 +  * 2- Création de sous programmes : avancer , reculer, tourner droite 90° , tourner gauche 90°, stop,  test distance
 +  * 3- avancer jusqu'à distance obstacle < 3 cm 
 +  * 4- si distance < 3 cm => stop 
 +  * 5- Tourner à droite => test distance obstacle
 +  * 6- si distance > 20 => avancer jusqu'à distance obstacle < 3 cm 
 +  * 7- sinon si distance < 3 cm => tourner encore à droite ( en tout depuis l'arret = 180 °)
 +  * 8- test rotation si rotation => 180°  
 +  * 8- tourner encore de 180° ( 2 fois tourner droite) 
 +  * 8- si distance > 20 => avancer
 +
 +
 +{{ :vittascience:capture_d_ecran_du_2026-03-06_16-23-12.png?direct&600 |}}
 +
 +
 +{{ :vittascience:capture_d_ecran_du_2026-03-06_16-17-09.png?direct&600 |}}
 +
 +<code c laby0033.ino>
 +#include <MeMCore.h>
 +#include <Arduino.h>
 +#include <Wire.h>
 +#include <SoftwareSerial.h>
 +
 +MeDCMotor motor_L(9);
 +MeDCMotor motor_R(10);
 +// Ultrasonic on PORT_3
 +MeUltrasonicSensor ultrasonic_3(PORT_3);
 +
 +int distancemax;
 +int pause;
 +int vitesse;
 +
 +void mBot_setMotorLeft(int8_t dir, int16_t speed) {
 +  speed = speed/100.0*255;
 +  motor_L.run((9) == M1 ? -(dir*speed) : (dir*speed));
 +}
 +
 +void mBot_setMotorRight(int8_t dir, int16_t speed) {
 +  speed = speed/100.0*255;
 +  motor_R.run((10) == M1 ? -(dir*speed) : (dir*speed));
 +}
 +
 +void avancer() {
 +  while (ultrasonic_3.distanceCm() >= distancemax) {
 +    mBot_setMotorRight(1, vitesse);
 +    mBot_setMotorLeft(1, vitesse);
 +  }
 +  mBot_setMotorRight(0, 0);
 +  mBot_setMotorLeft(0, 0);
 +  delay(1000*pause);
 +}
 +
 +void droite() {
 +  mBot_setMotorRight(-1, vitesse);
 +  mBot_setMotorLeft(1, vitesse);
 +  delay(1000*pause);
 +  mBot_setMotorRight(0, 0);
 +  mBot_setMotorLeft(0, 0);
 +}
 +
 +void gauche() {
 +  mBot_setMotorLeft(-1, vitesse);
 +  mBot_setMotorRight(1, vitesse);
 +  delay(1000*pause);
 +  mBot_setMotorRight(0, 0);
 +  mBot_setMotorLeft(0, 0);
 +}
 +
 +
 +void setup() {
 +  distancemax = 3;
 +  pause = 1;
 +  vitesse = 22;
 +  avancer();
 +  droite();
 +  avancer();
 +  gauche();
 +  avancer();
 +  gauche();
 +  avancer();
 +  droite();
 +  mBot_setMotorRight(1, 50);
 +  mBot_setMotorLeft(1, 50);
 +  delay(1000*1);
 +  mBot_setMotorRight(0, 0);
 +  mBot_setMotorLeft(0, 0);
 +}
 +
 +void loop() {
 +}
 +</code>
 +
 +
 +==== Code à Tester ====
 <code c Laby001.ino> <code c Laby001.ino>
 int capteurAvant = A0; int capteurAvant = A0;
Ligne 274: Ligne 371:
 } }
 </code> </code>
 +
 +<code c laby004.ino>
 +// =======================================================
 +// ROBOT LABYRINTHE PRO - FLOODFILL MICROMOUSE
 +// Arduino + L298N + capteurs distance
 +// =======================================================
 +
 +#define SIZE 16
 +
 +// -----------------------------
 +// MOTEURS
 +// -----------------------------
 +
 +const int ENA = 5;
 +const int IN1 = 6;
 +const int IN2 = 7;
 +
 +const int ENB = 9;
 +const int IN3 = 10;
 +const int IN4 = 11;
 +
 +// -----------------------------
 +// CAPTEURS
 +// -----------------------------
 +
 +const int trigFront = 2;
 +const int echoFront = 3;
 +
 +const int trigLeft = 4;
 +const int echoLeft = 8;
 +
 +const int trigRight = 12;
 +const int echoRight = 13;
 +
 +// -----------------------------
 +// STRUCTURE LABYRINTHE
 +// -----------------------------
 +
 +struct Cell
 +{
 +  bool wallN;
 +  bool wallE;
 +  bool wallS;
 +  bool wallW;
 +
 +  int distance;
 +  bool visited;
 +};
 +
 +Cell maze[SIZE][SIZE];
 +
 +// position robot
 +
 +int posX = 0;
 +int posY = 0;
 +
 +// orientation
 +// 0 = NORD
 +// 1 = EST
 +// 2 = SUD
 +// 3 = OUEST
 +
 +int direction = 0;
 +
 +// centre objectif
 +
 +int goalX = 7;
 +int goalY = 7;
 +
 +// -----------------------------
 +// SETUP
 +// -----------------------------
 +
 +void setup()
 +{
 +
 +  Serial.begin(9600);
 +
 +  pinMode(IN1, OUTPUT);
 +  pinMode(IN2, OUTPUT);
 +  pinMode(IN3, OUTPUT);
 +  pinMode(IN4, OUTPUT);
 +
 +  pinMode(ENA, OUTPUT);
 +  pinMode(ENB, OUTPUT);
 +
 +  pinMode(trigFront, OUTPUT);
 +  pinMode(echoFront, INPUT);
 +
 +  pinMode(trigLeft, OUTPUT);
 +  pinMode(echoLeft, INPUT);
 +
 +  pinMode(trigRight, OUTPUT);
 +  pinMode(echoRight, INPUT);
 +
 +  analogWrite(ENA, 160);
 +  analogWrite(ENB, 160);
 +
 +  initMaze();
 +  floodFill();
 +}
 +
 +// -----------------------------
 +// LOOP PRINCIPAL
 +// -----------------------------
 +
 +void loop()
 +{
 +
 +  scanWalls();
 +
 +  maze[posX][posY].visited = true;
 +
 +  floodFill();
 +
 +  int bestDir = bestDirection();
 +
 +  moveRobot(bestDir);
 +}
 +
 +// =======================================================
 +// INITIALISATION LABYRINTHE
 +// =======================================================
 +
 +void initMaze()
 +{
 +
 +  for (int x = 0; x < SIZE; x++)
 +  {
 +    for (int y = 0; y < SIZE; y++)
 +    {
 +
 +      maze[x][y].wallN = false;
 +      maze[x][y].wallE = false;
 +      maze[x][y].wallS = false;
 +      maze[x][y].wallW = false;
 +
 +      maze[x][y].visited = false;
 +
 +      maze[x][y].distance = abs(goalX - x) + abs(goalY - y);
 +    }
 +  }
 +}
 +
 +// =======================================================
 +// FLOODFILL
 +// =======================================================
 +
 +void floodFill()
 +{
 +
 +  bool change = true;
 +
 +  while (change)
 +  {
 +
 +    change = false;
 +
 +    for (int x = 0; x < SIZE; x++)
 +    {
 +      for (int y = 0; y < SIZE; y++)
 +      {
 +
 +        int minNeighbour = 1000;
 +
 +        if (!maze[x][y].wallN && y < SIZE - 1)
 +          minNeighbour = min(minNeighbour, maze[x][y + 1].distance);
 +
 +        if (!maze[x][y].wallE && x < SIZE - 1)
 +          minNeighbour = min(minNeighbour, maze[x + 1][y].distance);
 +
 +        if (!maze[x][y].wallS && y > 0)
 +          minNeighbour = min(minNeighbour, maze[x][y - 1].distance);
 +
 +        if (!maze[x][y].wallW && x > 0)
 +          minNeighbour = min(minNeighbour, maze[x - 1][y].distance);
 +
 +        if (maze[x][y].distance != minNeighbour + 1)
 +        {
 +          maze[x][y].distance = minNeighbour + 1;
 +          change = true;
 +        }
 +      }
 +    }
 +  }
 +}
 +
 +// =======================================================
 +// CHOIX DIRECTION OPTIMALE
 +// =======================================================
 +
 +int bestDirection()
 +{
 +
 +  int best = 1000;
 +  int bestDir = direction;
 +
 +  int nx, ny;
 +
 +  for (int d = 0; d < 4; d++)
 +  {
 +
 +    nx = posX;
 +    ny = posY;
 +
 +    if (d == 0)
 +      ny++;
 +    if (d == 1)
 +      nx++;
 +    if (d == 2)
 +      ny--;
 +    if (d == 3)
 +      nx--;
 +
 +    if (nx < 0 || ny < 0 || nx >= SIZE || ny >= SIZE)
 +      continue;
 +
 +    if (isWall(d))
 +      continue;
 +
 +    if (maze[nx][ny].distance < best)
 +    {
 +      best = maze[nx][ny].distance;
 +      bestDir = d;
 +    }
 +  }
 +
 +  return bestDir;
 +}
 +
 +// =======================================================
 +// MOUVEMENT ROBOT
 +// =======================================================
 +
 +void moveRobot(int dir)
 +{
 +
 +  int turn = dir - direction;
 +
 +  if (turn == -3)
 +    turn = 1;
 +  if (turn == 3)
 +    turn = -1;
 +
 +  if (turn == 1)
 +    turnRight();
 +
 +  if (turn == -1)
 +    turnLeft();
 +
 +  if (turn == 2 || turn == -2)
 +  {
 +    turnRight();
 +    turnRight();
 +  }
 +
 +  forward();
 +
 +  direction = dir;
 +
 +  updatePosition();
 +}
 +
 +// =======================================================
 +// POSITION
 +// =======================================================
 +
 +void updatePosition()
 +{
 +
 +  if (direction == 0)
 +    posY++;
 +
 +  if (direction == 1)
 +    posX++;
 +
 +  if (direction == 2)
 +    posY--;
 +
 +  if (direction == 3)
 +    posX--;
 +}
 +
 +// =======================================================
 +// DETECTION MURS
 +// =======================================================
 +
 +void scanWalls()
 +{
 +
 +  if (wallFront())
 +    setWall(direction);
 +
 +  if (wallLeft())
 +    setWall((direction + 3) % 4);
 +
 +  if (wallRight())
 +    setWall((direction + 1) % 4);
 +}
 +
 +void setWall(int dir)
 +{
 +
 +  if (dir == 0)
 +    maze[posX][posY].wallN = true;
 +
 +  if (dir == 1)
 +    maze[posX][posY].wallE = true;
 +
 +  if (dir == 2)
 +    maze[posX][posY].wallS = true;
 +
 +  if (dir == 3)
 +    maze[posX][posY].wallW = true;
 +}
 +
 +bool isWall(int dir)
 +{
 +
 +  if (dir == 0)
 +    return maze[posX][posY].wallN;
 +
 +  if (dir == 1)
 +    return maze[posX][posY].wallE;
 +
 +  if (dir == 2)
 +    return maze[posX][posY].wallS;
 +
 +  if (dir == 3)
 +    return maze[posX][posY].wallW;
 +
 +  return false;
 +}
 +
 +// =======================================================
 +// CAPTEURS DISTANCE
 +// =======================================================
 +
 +bool wallFront()
 +{
 +  return distance(trigFront, echoFront) < 15;
 +}
 +
 +bool wallLeft()
 +{
 +  return distance(trigLeft, echoLeft) < 15;
 +}
 +
 +bool wallRight()
 +{
 +  return distance(trigRight, echoRight) < 15;
 +}
 +
 +int distance(int trigPin, int echoPin)
 +{
 +
 +  digitalWrite(trigPin, LOW);
 +  delayMicroseconds(2);
 +
 +  digitalWrite(trigPin, HIGH);
 +  delayMicroseconds(10);
 +
 +  digitalWrite(trigPin, LOW);
 +
 +  long duration = pulseIn(echoPin, HIGH);
 +
 +  int d = duration * 0.034 / 2;
 +
 +  return d;
 +}
 +
 +// =======================================================
 +// MOTEURS
 +// =======================================================
 +
 +void forward()
 +{
 +
 +  digitalWrite(IN1, HIGH);
 +  digitalWrite(IN2, LOW);
 +
 +  digitalWrite(IN3, HIGH);
 +  digitalWrite(IN4, LOW);
 +
 +  delay(300);
 +
 +  stopMotors();
 +}
 +
 +void turnLeft()
 +{
 +
 +  digitalWrite(IN1, LOW);
 +  digitalWrite(IN2, HIGH);
 +
 +  digitalWrite(IN3, HIGH);
 +  digitalWrite(IN4, LOW);
 +
 +  delay(350);
 +
 +  stopMotors();
 +}
 +
 +void turnRight()
 +{
 +
 +  digitalWrite(IN1, HIGH);
 +  digitalWrite(IN2, LOW);
 +
 +  digitalWrite(IN3, LOW);
 +  digitalWrite(IN4, HIGH);
 +
 +  delay(350);
 +
 +  stopMotors();
 +}
 +
 +void stopMotors()
 +{
 +
 +  digitalWrite(IN1, LOW);
 +  digitalWrite(IN2, LOW);
 +
 +  digitalWrite(IN3, LOW);
 +  digitalWrite(IN4, LOW);
 +
 +  delay(100);
 +}
 +
 +// =======================================================
 +// DEBUG
 +// =======================================================
 +
 +void printMaze()
 +{
 +
 +  for (int y = SIZE - 1; y >= 0; y--)
 +  {
 +
 +    for (int x = 0; x < SIZE; x++)
 +    {
 +
 +      Serial.print(maze[x][y].distance);
 +      Serial.print("\t");
 +    }
 +
 +    Serial.println();
 +  }
 +
 +  Serial.println();
 +}
 +
 +</code>
 +
 +
  
 [[https://interstices.info/lalgorithme-de-pledge/|lalgorithme-de-pledge]] [[https://interstices.info/lalgorithme-de-pledge/|lalgorithme-de-pledge]]
/home/chanteri/www/fablab37110/data/attic/vittascience/labyrinthe.1772770441.txt.gz · Dernière modification : de admin