Published May 15, 2020 © 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,546

Things used in this project

Hardware components

ELEGOO Mega 2560 R3 Board Black ATmega2560 ATMEGA16U2

OLED I2C 128x64 (AZ Delivery)

SG90 Micro-servo motor

4x4 Matrix Array Keypad

there are a couple of suppliers on the marked. the code/project works with any kind of brand

M5 screw

M5 screw to mount the table

M5 nuts

Hardware, Washer

any kind of washer which is fitting easily on a M5

Jumper wires (generic)

3 layer cardboard 12mm

2 layer cardboard

i used a moving box

Hand tools and fabrication machines

Hot glue gun (generic)

Carpet Cutter

Story

Idea

I got inspired from Ahmed Azouz project, because it is a great mix of mechanical engineering and programming. But I didn't want to just copy his work, I wanted to use a new functional approach.

Approach

My approach was to write a solving algorithm to find a way trough each labyrinth and calculate the parameter for the 2 servo motors (X and Y-axis) to control the X/Y platforms in terms of moving the marble through the labyrinth.

Setup

I choose a size of 12cmx12cm (each square 1cmx1cm) for the maze. Any other size would work as well and can be adjusted in the header of the code. Anyway 12x12 is big enough to show the principle of the algorithm and it's practicable, because you've to glue together each maze.

Construction of the maze

I used a print template with 1cmx1cm rectangles. Then I draw a maze on it. There should be at least one solution/way from the start point (S) to the end point (E).

Base plate of each maze 12cmx12cm

The next step is to glue cardboard to the hatched area (wall). The height should be between 0.5-1cm. Make sure the ball is able to pass all the way through, esp. the edges.

Maze#2. Complete assembled.

Construction of the machine

The main building material is cardboard. Over the time I made great experience with these 12mm 3 layer cardboard. It's easy to handle and robust. For this project I used an OLED I2C 128x64, because I never used one before and I was excited to use of of them. The use of the OLED requested some libraries. I choose the ones from Adafruit. They are easy to handle. Nevertheless the huge size requested a MEGA board. If you want to use a standard LCD w/o any animations an Arduino UNO will be fine. As an input device I used a standard 4x4 keypad which will also take the part to control the marble in the manual mode.

Base plate with Keypad and Arduino board.

Base plate with glued keypad and recess for Arduino

The next step was to build the movable platforms for x- and y-dimension. The standard stepper motor SG90 is totally fine for that use case. The 12mm cardboard keeps them in a stable position. Hot glue was used as well. The rotation axis of the motor should be exactly in the middle of the side wall resp. platform.

Rear side wall of the Y-axis platform with SG90 motor for the X-axis platform

Assembly of the X-axis platform into the Y-axis platform

Be sure to make the holes for the motor exactly in the middle of the sidewall at least on the x-axis.

Mounting the Y-axis platform to the main sidewalls of the base plate.

The Y-axis platform was mounted on the left side to the SG-90 pinion. On the right side I made a hole with appr. 6-7mm diameter to ensure a M5 screw fits through and can turn easily, but not too easily. Between the walls you should use some washers. The final step is the assembly of the X-axis platform. Now both platforms can be turn independently from each other.

Almost finished.

The maze can be placed into the x-axis platform.

Final machine.

Some few data to the dimensions.

dimension in (cm)             X    Y    Z 
Base plate:                  30   26    -
Side walls (on the bottom):  -    18   20
Side walls (on the top):     -     8   20
Y-axis-platform (outer d.):  15  16.5   6
X-axis-platform (outer d.):  13   13    4

Final machine in auto mode.

Algorithm

The cardboard labyrinth is binary coded with zeros and ones (0 - wall; 1 - space). You can encode any labyrinth. For calculating the path from the staring point (S) to the end position (E) I used the left-hand-algorithm. This algorithm may not find the shortest way. In the next software version I will consider different algorithms. After the way is calculated the motor control parameters has to be determined. To keep it easy the angle to tilt each platform was chosen was defined with 10 degree in each direction. The other parameter was the time to keep the platform on -10/0/+10 degree. This time is depend on the previous movement of the marble. If the marble was already going in one specific direction the time for the next step in the same direction was shorter, because the marble was already accelerated to a specific speed.

Perspective

The labyrinth should be recorded automatically with the Pixy2 camera. So far the labyrinth is described with binary code (0/1) in a matrix. Afterwards the path is calculated (is already implemented now) and the ball is controlled. A new approach would be to monitor the speed and position of the ball with the camera and to adjust the motor parameters if necessary like a simple version of machine learning. One target could be the fastest possible speed respectively the shortest time from start to end.

Code

//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);
 
}

//---------------------------
//------ 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

3 projects • 1 follower

originally studied computer science; started programming games on U880; programming iOS apps; since a couple of weeks sticked into Arduino

Contact

Thanks to Limor Fried (Adafruit).

Comments

Please log in or sign up to comment.

Automatic marble labyrinth solver

Things used in this project

Hardware components

Hand tools and fabrication machines

Story

Idea

Approach

Setup

Construction of the maze

Construction of the machine

Algorithm

Perspective

Schematics

Fritzing

Code

Calibrate the SG90

Main Program

Credits

Michael Engel

Comments

Embed the widget on your own site

Automatic marble labyrinth solver

Automatic marble labyrinth solver

Things used in this project

Hardware components

Hand tools and fabrication machines

Story

Idea

Approach

Setup

Construction of the maze

Construction of the machine

Algorithm

Perspective

Schematics

Fritzing

Code

Calibrate the SG90

Main Program

Credits

Michael Engel

Comments

Related channels and tags