Michael Engel
Published © GPL3+

Automatic marble labyrinth solver

Arduino controlled solver for a marble labyrinth. The algorithm will controll the X- and Y-axis of the maze and moves the marble to the end.

IntermediateWork in progress15 hours1,463
Automatic marble labyrinth solver

Things used in this project

Hardware components

ELEGOO Mega 2560 R3 Board Black ATmega2560 ATMEGA16U2
ELEGOO Mega 2560 R3 Board Black ATmega2560 ATMEGA16U2
×1
OLED I2C 128x64 (AZ Delivery)
×1
SG90 Micro-servo motor
SG90 Micro-servo motor
×2
4x4 Matrix Array Keypad
there are a couple of suppliers on the marked. the code/project works with any kind of brand
×1
M5 screw
M5 screw to mount the table
×2
M5 nuts
×1
Hardware, Washer
Hardware, Washer
any kind of washer which is fitting easily on a M5
×5
Jumper wires (generic)
Jumper wires (generic)
×1
3 layer cardboard 12mm
×1
2 layer cardboard
i used a moving box
×1

Hand tools and fabrication machines

Hot glue gun (generic)
Hot glue gun (generic)
Carpet Cutter

Story

Read more

Schematics

Fritzing

Code

Calibrate the SG90

Arduino
use this program to evaluate the relative position of the SG90 when platform is even leveled. and take these parameters into the main program
//please COPY these values from the test program
#define STEPPER_BASE_X 94
#define STEPPER_BASE_Y 86
//Stepper Motor Calibration for Maze Project
//after assembly of the cardboard maze esp. mounting rotation tables to the stepper motor
//its neccessary to calibrate it to an ven position
//90 degree was the optimal target for the Maze Solver Programm

//Procedure:
//1. Before assembling the rotation tables to the stepper motor the stepper has to be rotate via this programm to 90 degree
//2. Assembling the rotation tables to the pinion of the motor -> you will realize that the pinion resp. the motor will move a little bit during the fitting of plastic plate to the pinion due to the fitting of the gears
//3. start this programm again and try out which angle appr. 80-100 will bring both tables back to an even position 

#include <Servo.h>

#define PWM_STEPPER_X     11
#define PWM_STEPPER_Y     10

Servo Stepper_X;
Servo Stepper_Y;

void setup() {
  
  Stepper_X.attach(PWM_STEPPER_X);  // attaches the servo on pin 9 to the servo object
  Stepper_Y.attach(PWM_STEPPER_Y);  // attaches the servo on pin 9 to the servo object
  Serial.begin(9600);
}

void loop() {
  //in my case these values will bring both tables (X and Y) to an even position
   Stepper_X.write(94); 
   Stepper_Y.write(86); 
   delay(1000);
 
}

Main Program

Arduino
just copy/paste into your Arduino IDE
//---------------------------
//------ Maze Solver v1 -----
//algorithm based on: left hand algorithm
//Code: Michael Engel
//Project start: 27APR2020
//last update:   10MAY2020
//Left hand algorithm
//not implemented yet: manual calabration
//
//Note: the Serial.print commands can be deleted -> used for debugging

#include<stdio.h>
#include <SPI.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <Keypad.h>
#include <Servo.h>

#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels

// Declaration for an SSD1306 display connected to I2C (SDA, SCL pins)
#define OLED_RESET     4 // Reset pin # (or -1 if sharing Arduino reset pin)
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

#define MAXNODES 144
#define ROW 12 // (y axis)
#define COL 12 // (x axis)

#define PWM_STEPPER_X     11
#define PWM_STEPPER_Y     10

//please COPY these values from the test programm
#define STEPPER_BASE_X    94
#define STEPPER_BASE_Y    86
#define MAX_TILTING       10  //maximale Kippung der Tische (Recommended 10 degree)

typedef struct node {
  int x;
  int y;
} node;

typedef struct motor_commands {
  int angle;
  int zeit;
} motor_commands;

int grid[ROW][COL];
int maze = 1;
String grid_row[12];

//Current Position of marble
int current_x = 10;
int current_y = 10;

//Start Point of the marble
int start_x = 10;
int start_y = 10;

//End_Point of the maze
int end_x = 1;
int end_y = 1;

//Orientation of marble
//0 - right; 90 - up; 180 - left; 270 - down
int orientation_marble = 90; // left direction

//save the path from start to end
node path[MAXNODES];

//save the optimized path (w/o dead end) from start to end
node opt_path[MAXNODES];

//-------
Servo Stepper_X;
Servo Stepper_Y;

//---
motor_commands stepper_x[MAXNODES];
motor_commands stepper_y[MAXNODES];

//maximale und minimale Kippung der Tische X und Y
int max_angle_x;
int min_angle_x;
int max_angle_y;
int min_angle_y;

int node_index = 0; //latest position of the path resp. max. number of nodes
int opt_node_index = 0;

int mode = 0;    //flag for solving mode (0=manual; 1-auto)
//define mode of the softness of the table movement during manual mode
int move_mode = 0; // 0 - hard , 1 - soft

//Define size of KeyPad
const byte COLS = 4; //4 col
const byte ROWS = 4; //4 rows

//Die Ziffern und Zeichen des Keypads werden eingegeben:
char hexaKeys[ROWS][COLS] = {
  {'D', '#', '0', '*'},
  {'C', '9', '8', '7'},
  {'B', '6', '5', '4'},
  {'A', '3', '2', '1'}
};

byte colPins[COLS] = {2, 3, 4, 5}; //Definition der Pins für die 4 Spalten
byte rowPins[ROWS] = {6, 7, 8, 9}; //Definition der Pins für die 4 Zeilen
char Taste; //Taste ist die Variable für die jeweils gedrückte Taste.

Keypad Tastenfeld = Keypad(makeKeymap(hexaKeys), rowPins, colPins, ROWS, COLS); //Das Keypad kann absofort mit "Tastenfeld" angesprochen werden

void setup()
{

  Serial.begin(9600);
  Stepper_X.attach(PWM_STEPPER_X);  // attaches the servo on pin 9 to the servo object
  Stepper_Y.attach(PWM_STEPPER_Y);  // attaches the servo on pin 9 to the servo object

  //calculating max und min Tilting area of the X/Y tables
  max_angle_x = STEPPER_BASE_X + MAX_TILTING;
  min_angle_x = STEPPER_BASE_X - MAX_TILTING;
  max_angle_y = STEPPER_BASE_Y + MAX_TILTING;
  min_angle_y = STEPPER_BASE_Y - MAX_TILTING;


  Serial.println();        Serial.println();        Serial.println();
  Serial.println("****************************************");
  Serial.println("Initialize grid structure of maze");
  Serial.print("max_X");
  Serial.println(max_angle_x);
  Serial.print("min_X");
  Serial.println(min_angle_x);
  Serial.print("max_Y");
  Serial.println(max_angle_y);
  Serial.print("min_Y");
  Serial.println(min_angle_y);
  // SSD1306_SWITCHCAPVCC = generate display voltage from 3.3V internally
  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { // Address 0x3D for 128x64
    Serial.println(F("SSD1306 allocation failed"));
    for (;;); // Don't proceed, loop forever
  }
  // Show initial display buffer contents on the screen --
  // the library initializes this with an Adafruit splash screen.
  display.display();

  // Clear the buffer

  // Show the display buffer on the screen. You MUST call display() after
  // drawing commands to make them visible on screen!

  // display.display() is NOT necessary after every single drawing command,
  // unless that's what you want...rather, you can batch up a bunch of
  // drawing operations and then update the screen all at once by calling
  // display.display(). These examples demonstrate both approaches...

}

void getGrid(int maze)
{
  //Maze has a grid of 12x12
  //0 = wall; 1 = space

  String maze_unit; //ein Element aus dem Maze -> als Zwischenspeicher

  if (maze == 1)
  {
    // Test Set #1
    //grid x,y
    grid_row [0] = "000000000000";
    grid_row [1] = "010111011110";
    grid_row [2] = "010101110010";
    grid_row [3] = "011101000110";
    grid_row [4] = "000001011100";
    grid_row [5] = "011111010000";
    grid_row [6] = "010000011110";
    grid_row [7] = "010111000000";
    grid_row [8] = "010101011110";
    grid_row [9] = "011101001010";
    grid_row [10] = "010001111010";
    grid_row [11] = "000000000000";

  }
  else if (maze == 2)
  {
    //Testset 2
    //grid x,y
    grid_row [0] = "000000000000";
    grid_row [1] = "010100001110";
    grid_row [2] = "010101111010";
    grid_row [3] = "010101000010";
    grid_row [4] = "010101011110";
    grid_row [5] = "011111010000";
    grid_row [6] = "000000011110";
    grid_row [7] = "011100100010";
    grid_row [8] = "010111101110";
    grid_row [9] = "010000001010";
    grid_row [10] = "011111111010";
    grid_row [11] = "000000000000";
  }
  else if (maze == 3)
  {
    //Testset 3
    //grid x,y
    grid_row [0] = "000000000000";
    grid_row [1] = "011111111110";
    grid_row [2] = "010000000010";
    grid_row [3] = "010000000010";
    grid_row [4] = "010000000010";
    grid_row [5] = "010000000010";
    grid_row [6] = "010000000010";
    grid_row [7] = "010000000010";
    grid_row [8] = "010000000010";
    grid_row [9] = "010000000010";
    grid_row [10] = "011111111110";
    grid_row [11] = "000000000000";
  }

  //Maze in die GRID_Structure einlesen
  for (int i = 0; i < ROW; i++) {
    for (int j = 0; j < COL; j++) {
      maze_unit = grid_row[i].charAt(j);
      grid[i][j] = maze_unit.toInt();
      Serial.println(grid[i][j]);
    }
  }
}

void man_mode()
{
  //manuel mode
  display.clearDisplay();
  //Request for manuel balancing
  display.setTextSize(2);             // Normal 1:1 pixel scale
  display.setTextColor(SSD1306_BLACK, SSD1306_WHITE); // Draw 'inverse' text
  display.setCursor(0, 0);            // Start at top-left corner
  display.println(F("Manuel"));
  display.setCursor(0, 18);            // Start at top-left corner
  display.println(F("Mode"));
  display.setTextSize(1);             // Normal 1:1 pixel scale
  display.setCursor(0, 40);
  display.setTextColor(SSD1306_WHITE);        // Draw white text
  display.println("Press any key ");
  display.println("to start.");
  display.display();

  Taste = Tastenfeld.getKey(); //Mit Unter der Variablen pressedKey entspricht der gedrückten Taste
  while (!Taste)
  { //Wenn eine Taste gedrückt wurde
    Taste = Tastenfeld.getKey(); //Mit Unter der Variablen pressedKey entspricht der gedrückten Taste
    if (Taste)
    {
      Serial.println("Taste gedrueckt!");
      break;
    }
  }
  display.clearDisplay();

  // Draw GUI
  display.drawLine(32, 1, 32, 8, SSD1306_WHITE);
  display.drawLine(32, 1, 31, 2, SSD1306_WHITE);
  display.drawLine(32, 1, 33, 2, SSD1306_WHITE);

  display.drawLine(32, 63, 32, 55, SSD1306_WHITE);
  display.drawLine(32, 63, 31, 62, SSD1306_WHITE);
  display.drawLine(32, 63, 33, 62, SSD1306_WHITE);

  display.drawLine(1, 31, 8, 31, SSD1306_WHITE);
  display.drawLine(1, 31, 2, 30, SSD1306_WHITE);
  display.drawLine(1, 31, 2, 32, SSD1306_WHITE);

  display.drawLine(54, 31, 61, 31, SSD1306_WHITE);
  display.drawLine(60, 30, 61, 31, SSD1306_WHITE);
  display.drawLine(60, 32, 61, 31, SSD1306_WHITE);

  //text
  display.setTextSize(1);             // Normal 1:1 pixel scale
  display.setTextColor(SSD1306_WHITE); // Draw 'inverse' text
  display.setCursor(30, 13);            // Start at top-left corner
  display.println(F("2"));
  display.setCursor(30, 46);            // Start at top-left corner
  display.println(F("8"));
  display.setCursor(13, 29);            // Start at top-left corner
  display.println(F("4"));
  display.setCursor(30, 29);            // Start at top-left corner
  display.println(F("5"));
  display.setCursor(47, 29);            // Start at top-left corner
  display.println(F("6"));

  display.setCursor(72, 45);
  display.println("Press #");
  display.setCursor(72, 55);
  display.println("for STOPP");
  //hier muessen noch Pfeile gemalt werden

  display.display();
  if (move_mode == 0)
  {

    //hard movement - table is moving directly to max/min position resp. is moving via middle position

    //Zwischenspeicher der Position -1, 0, +1 (0 - Mittelstellung)
    int x = 0;
    int y = 0;

    Taste = Tastenfeld.getKey(); //Mit Unter der Variablen pressedKey entspricht der gedrückten Taste
    while (Taste != '#')
    { //Wenn eine Taste gedrückt wurde
      Taste = Tastenfeld.getKey(); //Mit Unter der Variablen pressedKey entspricht der gedrückten Taste

      Serial.print("Die Taste ");
      Serial.print(Taste);
      Serial.print(" wurde gedrueckt");

      if (Taste == '2')
      {
        Serial.println("hoch");
        if  (y == 1)
        {
          //Zwischenschritt ueber Mittelstellung
          y = 0;
          Stepper_Y.write(STEPPER_BASE_Y);
        }
        else if (y == 0)
        {
          y = -1;
          Stepper_Y.write(min_angle_y);
        }
      }
      else if (Taste == '4')
      {
        Serial.println("links");
        if (x == -1)
        {
          x = 0;
          Stepper_X.write(STEPPER_BASE_X);
        }
        else if (x == 0)
        {
          x = 1;
          Stepper_X.write(max_angle_x);
        }
      }
      else if (Taste == '6')
      {
        Serial.println("rechts");
        if (x == 1)
        {
          x = 0;
          Stepper_X.write(STEPPER_BASE_X);
        }
        else if (x == 0)
        {
          x = -1;
          Stepper_X.write(min_angle_x);
        }


      }
      else if (Taste == '8')
      {
        Serial.println("runter");
        //Stepper_X.write(STEPPER_BASE_X);
        if (y == -1)
        {
          y = 0;
          Stepper_Y.write(STEPPER_BASE_Y);

          // delay(100);
        }
        else if (y == 0)
        {

          y = 1;
          Stepper_Y.write(max_angle_y);
        }
      }
      else if (Taste == '5')
      {
        Serial.println("neutral");
        Stepper_X.write(STEPPER_BASE_X);
        Stepper_Y.write(STEPPER_BASE_Y);
        // delay(100);

      }

      else if (Taste == '#')
      {
        Serial.println("abroad");
        break;
      }
      else
      {
        // bring tables back to neutral posistion
        /*
          Stepper_X.write(STEPPER_BASE_X);
          Stepper_Y.write(STEPPER_BASE_Y);
        */
      }

    }
  }
  else if (move_mode == 1)
  {
    //soft movement
    int x = STEPPER_BASE_X;
    int y = STEPPER_BASE_Y;
    char previousPressedKey;

    Taste = Tastenfeld.getKey(); //Mit Unter der Variablen pressedKey entspricht der gedrückten Taste
    while (Taste != "#")
    { //Wenn eine Taste gedrückt wurde

      Taste = Tastenfeld.getKey(); //Mit Unter der Variablen pressedKey entspricht der gedrückten Taste
      KeyState state = Tastenfeld.getState();
      //Serial.print("Die Taste ");
      Serial.print(Taste);
      Serial.println(state);

      if (state == PRESSED && Taste != NO_KEY)
      {
        previousPressedKey = Taste;
        if (Taste == '2')
        {
          Serial.println("hoch");
          y--;
          if (y < min_angle_y) y = min_angle_y;
          Stepper_X.write(x);
          Stepper_Y.write(y);
          //  delay(100);
        }
        else if (Taste == '4')
        {
          Serial.println("links");
          x++;
          if (x > max_angle_x) x = max_angle_x;

          Stepper_X.write(x);
          Stepper_Y.write(y);
          // delay(100);
        }
        else if (Taste == '6')
        {
          Serial.println("rechts");
          x--;
          if (x < min_angle_x)  x = min_angle_x;
          Stepper_X.write(x);
          Stepper_Y.write(y);
          // delay(100);

        }
        else if (Taste == '8')
        {
          Serial.println("runter");
          y++;
          if (y > max_angle_y) y = max_angle_y;
          Stepper_X.write(x);
          Stepper_Y.write(y);
          // delay(100);

        }
        else if (Taste == '5')
        {
          Serial.println("neutral");
          //soft den Mittelpunkt ansteuern
          x = STEPPER_BASE_X;
          y = STEPPER_BASE_Y;
          Stepper_X.write(STEPPER_BASE_X);
          Stepper_Y.write(STEPPER_BASE_Y);
          //  delay(100);

        }

      }

      //Serial.print(" wurde gedrueckt");
      if (state == HOLD)
      {

        Serial.print("HOLD");
        Serial.println(Taste);

        if (previousPressedKey == '2')
        {
          Serial.println("hoch");
          y--;
          if (y < min_angle_y) y = min_angle_y;
          Stepper_X.write(x);
          Stepper_Y.write(y);
          //  delay(100);
        }
        else if (previousPressedKey == '4')
        {
          Serial.println("links");
          x++;
          if (x > max_angle_x) x = max_angle_x;

          Stepper_X.write(x);
          Stepper_Y.write(y);
          //delay(100);
        }
        else if (previousPressedKey == '6')
        {
          Serial.println("rechts");
          x--;
          if (x < min_angle_x)  x = min_angle_x;
          Stepper_X.write(x);
          Stepper_Y.write(y);
          // delay(100);

        }
        else if (previousPressedKey == '8')
        {
          Serial.println("runter");
          y++;
          if (y > max_angle_y) y = max_angle_y;
          Stepper_X.write(x);
          Stepper_Y.write(y);
          // delay(100);

        }
        else if (previousPressedKey == '5')
        {
          Serial.println("neutral");
          //soft den Mittelpunkt ansteuern
          x = STEPPER_BASE_X;
          y = STEPPER_BASE_Y;
          Stepper_X.write(STEPPER_BASE_X);
          Stepper_Y.write(STEPPER_BASE_Y);
          //   delay(100);

        }

        else if (previousPressedKey == '#')
        {
          Serial.println("abroad");
          break;
        }
        else
        {
          // bring tables back to neutral posistion
          /*
            Stepper_X.write(STEPPER_BASE_X);
            Stepper_Y.write(STEPPER_BASE_Y);
          */
        }
      }
    }
  }


}

void auto_manu_mode()
{
  display.clearDisplay();
  //Request for manuel balancing
  display.setTextSize(2);             // Normal 1:1 pixel scale
  display.setTextColor(SSD1306_BLACK, SSD1306_WHITE); // Draw 'inverse' text
  display.setCursor(0, 0);            // Start at top-left corner
  display.println(F("Solving"));
  display.setCursor(0, 18);            // Start at top-left corner
  display.println(F("Mode"));
  display.setTextSize(1);             // Normal 1:1 pixel scale
  display.setCursor(0, 40);
  display.setTextColor(SSD1306_WHITE);        // Draw white text
  display.println("Select solving ");
  display.println("mode.");
  display.println("0-manu | 1-auto");
  display.display();

  Taste = Tastenfeld.getKey(); //Mit Unter der Variablen pressedKey entspricht der gedrückten Taste
  while (!Taste)
  { //Wenn eine Taste gedrückt wurde
    Taste = Tastenfeld.getKey(); //Mit Unter der Variablen pressedKey entspricht der gedrückten Taste
    Serial.print("Die Taste ");
    Serial.print(Taste);
    Serial.print(" wurde gedrueckt");

    if (Taste == '0')
    {
      Serial.println("manual mode selected");
      mode = 0;
      break;
    }
    if (Taste == '1')
    {
      Serial.println("auto mode selected");
      mode = 1;
      //jump to procedure for recalibration
      break;
    }
  }



}

void calibrate_table()
{
  //calibrate maze table if necessary
  //please place a marble into the maze and check if the maze is even

  testdrawrect_fast();
  display.clearDisplay();
  display.setTextSize(2);             // Normal 1:1 pixel scale
  display.setTextColor(SSD1306_BLACK, SSD1306_WHITE); // Draw 'inverse' text
  display.setCursor(0, 0);            // Start at top-left corner
  display.println(F("Calibrate"));
  display.setCursor(0, 18);            // Start at top-left corner
  display.println(F("Table"));
  display.setTextSize(1);             // Normal 1:1 pixel scale
  display.setCursor(0, 40);
  display.setTextColor(SSD1306_WHITE);        // Draw white text
  display.println("Please place a ball");
  display.println("into the maze and");
  display.println("check if balanced.");
  display.display();

  //bring the X/Y motors in the center position -> both tables should be even
  Stepper_X.write(STEPPER_BASE_X);
  Stepper_Y.write(STEPPER_BASE_Y);

  testdrawrect_fast();
  display.clearDisplay();
  //Request for manuel balancing
  display.setTextSize(2);             // Normal 1:1 pixel scale
  display.setTextColor(SSD1306_BLACK, SSD1306_WHITE); // Draw 'inverse' text
  display.setCursor(0, 0);            // Start at top-left corner
  display.println(F("Calibrate"));
  display.setCursor(0, 18);            // Start at top-left corner
  display.println(F("Table"));
  display.setTextSize(1);             // Normal 1:1 pixel scale
  display.setCursor(0, 40);
  display.setTextColor(SSD1306_WHITE);        // Draw white text
  display.println("Do you need to ");
  display.println("recalibrate?");
  display.println("1-YES | 0-NO");
  display.display();

  Taste = Tastenfeld.getKey(); //Mit Unter der Variablen pressedKey entspricht der gedrückten Taste
  while (!Taste)
  { //Wenn eine Taste gedrückt wurde
    Taste = Tastenfeld.getKey(); //Mit Unter der Variablen pressedKey entspricht der gedrückten Taste
    Serial.print("Die Taste ");
    Serial.print(Taste);
    Serial.print(" wurde gedrueckt");

    if (Taste == '0')
    {
      Serial.println("no recalibration required");
      break;
    }
    if (Taste == '1')
    {
      Serial.println("recalibration required");
      //jump to procedure for recalibration
      break;
    }
  }
}

void choose_maze()
{
  display.clearDisplay();
  display.setTextSize(2);             // Normal 1:1 pixel scale
  display.setTextColor(SSD1306_BLACK, SSD1306_WHITE); // Draw 'inverse' text
  display.setCursor(0, 0);            // Start at top-left corner
  display.println(F("Select"));
  display.setCursor(0, 18);            // Start at top-left corner
  display.println(F("Maze"));
  display.setTextSize(1);             // Normal 1:1 pixel scale
  display.setCursor(0, 40);
  display.setTextColor(SSD1306_WHITE);        // Draw white text
  display.println("Press 1-5");
  display.display();

  Taste = Tastenfeld.getKey(); //Mit Unter der Variablen pressedKey entspricht der gedrückten Taste
  while (!Taste)
  { //Wenn eine Taste gedrückt wurde
    Taste = Tastenfeld.getKey(); //Mit Unter der Variablen pressedKey entspricht der gedrückten Taste
    Serial.print("Die Taste ");
    Serial.print(Taste);
    Serial.print(" wurde gedrueckt");

    if ((Taste == '1') || (Taste == '2') || (Taste == '3') || (Taste == '4') || (Taste == '5'))
    {
      maze = (int) Taste - '0';
      Serial.println(maze);

      delay(500);
      break;
    }
  }
}

void leftHandAlg()
{
  //Left hand algorithm

  display.clearDisplay();
  display.setTextSize(2);             // Normal 1:1 pixel scale
  display.setTextColor(SSD1306_BLACK, SSD1306_WHITE); // Draw 'inverse' text
  display.setCursor(0, 0);            // Start at top-left corner
  display.println(F("Calculating..."));
  display.display();

  //take start position to the path as position #1
  path[0].x = start_x;
  path[0].y = start_y;

  while ((current_x > end_x) || (current_y > end_y))
  {
    //Pruefen ob links eine Wand ist
    if (check_no_left_wall())
    {
      //on the left side is a wall
      rotate_CCW();
      step_forward();
    }
    else
    {
      //there is no wall on the left side
      if (check_no_front_wall())
      {
        step_forward();
      }
      else
      {
        rotate_CW();

      }
    }
  }
  Serial.println("End position reached.");

}

boolean check_no_left_wall()
{
  boolean left_wall = true;

  if (orientation_marble == 0)
  {
    if (grid[current_y - 1][current_x] == 0) left_wall = false;
  }
  else if (orientation_marble == 90)
  {
    if (grid[current_y][current_x - 1] == 0) left_wall = false;
  }
  else if (orientation_marble == 180)
  {
    if (grid[current_y + 1][current_x] == 0) left_wall = false;
  }
  else
  {
    if (grid[current_y][current_x + 1] == 0) left_wall = false;
  }


  return left_wall;
}

boolean check_no_front_wall()
{
  boolean front_wall = true;

  if (orientation_marble == 0)
  {
    if (grid[current_y][current_x + 1] == 0) front_wall = false;
  }
  else if (orientation_marble == 90)
  {
    if (grid[current_y - 1][current_x] == 0) front_wall = false;
  }
  else if (orientation_marble == 180)
  {
    if (grid[current_y][current_x - 1] == 0) front_wall = false;
  }
  else
  {
    if (grid[current_y + 1][current_x] == 0) front_wall = false;
  }


  return front_wall;
}
void rotate_CCW()
{
  Serial.println("CCW");
  orientation_marble = orientation_marble + 90;
  if (orientation_marble == 360) orientation_marble = 0;
}

void rotate_CW()
{
  Serial.println("CW");
  orientation_marble = orientation_marble - 90;
  if (orientation_marble == -90) orientation_marble = 270;
}

void step_forward()
{
  Serial.println("step_forward");
  if (orientation_marble == 0)
  {
    current_x = current_x + 1;
  }
  else if (orientation_marble == 90)
  {
    current_y = current_y - 1;
  }
  else if (orientation_marble == 180)
  {
    current_x = current_x - 1;
  }
  else
  {
    current_y = current_y + 1;
  }
  //save new position of the marble to the path
  node_index++;
  Serial.print("Node_index=");
  Serial.println(node_index);
  path[node_index].x = current_x;
  path[node_index].y = current_y;
  Serial.println(path[node_index].x);
  Serial.println(path[node_index].y);
}

void optimize_path()
{
  opt_node_index = node_index; //ist erstmal identisch, aber der Opt_node_index sollte ggf. kleiner werden
  //ich glaube man haette opt_path gar nicht gebraucht und haette die Optimierung auch mit dem Originaldaten von path[i] machen koennen
  //1.step temporary copy complete path based on left-hand alg to the array opt_path
  for (int i = 0; i <= node_index; i++)
  {
    opt_path[i].x = path[i].x;
    opt_path[i].y = path[i].y;
  }

  //step #2 search for dead ends in the opt_path
  for (int i = 0; i <= opt_node_index; i++)
  {
    for (int j = i + 1; j <= opt_node_index; j++)
    {
      if ((opt_path[i].x == opt_path[j].x) && (opt_path[i].y == opt_path[j].y) )
      {
        //doppelten Node gefunden
        //Bereich zwischen i und j loeschen und opt_node_index entsprechend reduzieren
        for (int k = i; k <= opt_node_index - (j - i); k++)
        {
          opt_path[k].x = opt_path[k + (j - i)].x;
          opt_path[k].y = opt_path[k + (j - i)].y;
          // opt_node_index=opt_node_index-(j-i)+1;
        }
        opt_node_index = opt_node_index - (j - i);
        Serial.print("opt_Node_index=");
        Serial.println(opt_node_index);
        Serial.print("j=");
        Serial.println(j);
        Serial.print("i=");
        Serial.println(i);
      }

    }
  }

}


void initdrawchar(void) {
  display.clearDisplay();

  display.setTextSize(2);             // Normal 1:1 pixel scale
  display.setTextColor(SSD1306_BLACK, SSD1306_WHITE); // Draw 'inverse' text
  display.setCursor(0, 0);            // Start at top-left corner
  display.println(F("Initialize"));
  display.setTextSize(1);             // Normal 1:1 pixel scale
  display.setTextColor(SSD1306_WHITE);        // Draw white text
  display.println("Loading maze ...");
  display.display();
  delay(1000);
}

void startdrawchar(void) {
  display.clearDisplay();

  display.setTextSize(2);             // Normal 1:1 pixel scale
  display.setTextColor(SSD1306_BLACK, SSD1306_WHITE); // Draw 'inverse' text
  display.setCursor(0, 0);            // Start at top-left corner
  display.println(F("MAZEsolverV1.0"));
  display.setTextSize(1);             // Normal 1:1 pixel scale
  display.setTextColor(SSD1306_WHITE);        // Draw white text
  display.println("Michael Engel");
  display.println("April/May 2020");
  display.display();
  delay(2000);
}

void testdrawroundrect(void) {
  display.clearDisplay();

  for (int16_t i = 0; i < display.height() / 2 - 2; i += 2) {
    display.drawRoundRect(i, i, display.width() - 2 * i, display.height() - 2 * i,
                          display.height() / 4, SSD1306_WHITE);
    display.display();
    delay(1);
  }

  delay(2000);
}
void testfillrect(void) {
  display.clearDisplay();

  for (int16_t i = 0; i < display.height() / 2; i += 3) {
    // The INVERSE color is used so rectangles alternate white/black
    display.fillRect(i, i, display.width() - i * 2, display.height() - i * 2, SSD1306_INVERSE);
    display.display(); // Update screen with each newly-drawn rectangle
    delay(1);
  }

  delay(2000);
}

void testfillroundrect(void) {
  display.clearDisplay();

  for (int16_t i = 0; i < display.height() / 2 - 2; i += 2) {
    // The INVERSE color is used so round-rects alternate white/black
    display.fillRoundRect(i, i, display.width() - 2 * i, display.height() - 2 * i,
                          display.height() / 4, SSD1306_INVERSE);
    display.display();
    delay(1);
  }

  delay(2000);
}

void testdrawrect(void) {
  display.clearDisplay();

  for (int16_t i = 0; i < display.height() / 2; i += 2) {
    display.drawRect(i, i, display.width() - 2 * i, display.height() - 2 * i, SSD1306_WHITE);
    display.display(); // Update screen with each newly-drawn rectangle
    delay(1);
    display.drawRect(i, i, display.width() - 2 * i, display.height() - 2 * i, SSD1306_BLACK);
    display.display();
    delay(1);
  }
  for (int16_t i = display.height() / 2; i > 0; i -= 2) {
    display.drawRect(i, i, display.width() - 2 * i, display.height() - 2 * i, SSD1306_WHITE);
    display.display(); // Update screen with each newly-drawn rectangle
    delay(1);
    display.drawRect(i, i, display.width() - 2 * i, display.height() - 2 * i, SSD1306_BLACK);
    display.display();
    delay(1);
  }


  delay(10);
}

void testdrawrect_fast(void) {
  display.clearDisplay();

  for (int16_t i = 0; i < display.height() / 2; i += 4) {
    display.drawRect(i, i, display.width() - 2 * i, display.height() - 2 * i, SSD1306_WHITE);
    display.display(); // Update screen with each newly-drawn rectangle
    display.drawRect(i, i, display.width() - 2 * i, display.height() - 2 * i, SSD1306_BLACK);
    display.display();
  }
  for (int16_t i = display.height() / 2; i > 0; i -= 4) {
    display.drawRect(i, i, display.width() - 2 * i, display.height() - 2 * i, SSD1306_WHITE);
    display.display(); // Update screen with each newly-drawn rectangle

    display.drawRect(i, i, display.width() - 2 * i, display.height() - 2 * i, SSD1306_BLACK);
    display.display();

  }


  delay(10);
}

void drawmaze(void) {
  display.clearDisplay();
  //Maze mittels Rechtecken malen ->
  for (int j = 0; j < ROW; j++) {
    for (int k = 0; k < COL; k++)
    {
      //draw a rectangle if wall identified
      if (grid[j][k] == 0)
      {
        display.fillRect((k * 5) + 3, (j * 5) + 3, 5, 5, SSD1306_INVERSE);
      }
    }
  }
...

This file has been truncated, please download it to see its full contents.

Credits

Michael Engel

Michael Engel

3 projects • 1 follower
originally studied computer science; started programming games on U880; programming iOS apps; since a couple of weeks sticked into Arduino
Thanks to Limor Fried (Adafruit).

Comments

Related channels and tags
  • Elegoo
  • Gaming
  • Robotics