Mouse in a labyrinth sketch

When it comes to labyrinths there are more challenges than simply avoiding walls in a box. The mouse needs to follow walls, when they are on both sides and when the wall is only one side. It also needs to detect when it reaches a turning with a wall in front of it, make the correct turn, and continue along the new hallway.

The code below is NOT perfect and my mouse lurches a bit, especially after turns and entering the new hall way. I have not yet found out why...

This sketch uses a different MouseSensors.h file to the box sketch so take care! It needs to do this as instead of the events being detected at a fixed distance (DETECT) two variables are use for the side and front distances. A further variable is used for the detection of walls. These variables are set by a call to the sketch calibrate() function. The mouse reads the side distances, then turns east and measures the front distance and the wall distance with one side absent.

I use a Bluetooth module connected to Arduino pins 0 & 1 to send data back to the Mac. I use the program CoolTerm on the Mac to display the readings from the function printsens().



Here's my mouse in a simple labyrinth:



and here's the code in three parts: the ".ino" sketch and the two ".h" class files for motors and sensors:

MOUSELABRTYNTH.INO

/* MouseLabrynth *** MASTE R FILE *** 16-7-14
his program tracks through a Labrynth - 
that is a twisted, closed pathway with the goal at the end  
 */

#include 
#include "MouseMotors.h"
#include "MouseSensors.h"

// `LED & button
#define LED 13
#define BUTTON 12

// proportional multiplier (float), KP1 for hall, KP2 for sides
#define KP1 0.25
#define KP2 2.5

MouseMotors motors; // motors object
MouseSensors sensors; // pass pin numbers to sensors object

// event returned by state()
byte event;

void setup()
{
  Serial.begin(9600);

  motors.attach(); // attach servos

  sensors.enable(); // enable emitter pins

  pinMode(BUTTON, INPUT_PULLUP); // button input
  pinMode(LED, OUTPUT); // LED output

  digitalWrite(LED, LOW);
  while(digitalRead(BUTTON)); // wait for button press
  delay(100);
  while(!digitalRead(BUTTON)); // wait for button release
  digitalWrite(LED, HIGH);

  // place mouse between walls to calibrate, then push button
  calibrate(); // get front, and side limits for sensors.state
}

void loop()
{ 
  printsens(); // temp DEBUG code

  labNav(sensors.state());
}

// Navigate the labarynth based on event returned by state() function
// events map
//     7
//  3  1  5
// 2   6   4
void labNav(byte event)
{ 
  switch(event) // get event
  {
  case 0: // no event or north only
  case 1:
    motors.halt();
    error();
    break;
  case 2: // wall to west
    fwdNav();
    break;
  case 3: // walls west & north
    motors.turn(E, 90);
    sensors.init();
    break;
  case 4: // wall est
    fwdNav();
    break;
  case 5: // walls west & east
    motors.turn(W, 90);
    sensors.init();
    break;
  case 6: // walls north & east
    fwdNav();
    break;
  case 7: // walls all round
    motors.halt();
    error();
    break;
  }
}

// go forwards checking sides
// if L&R stay in centre, if L or R follow it, if none go strait
// Threshold thset is found by calibrate() 
void fwdNav()
{

  while(sensors.north < sensors.front) // while no wall north
  {
    sensors.sense();

    if(sensors.west > sensors.wall && sensors.east > sensors.wall) // walls west & east
    {
      motors.fwd(0, KP1 * (float)(sensors.east - sensors.west)); // error is a float
    }
    else if(sensors.east < sensors.wall) // no east wall
    {
      motors.fwd(0, KP2 * (float)(sensors.side - sensors.west)); // follow west wall
    }
    else if(sensors.west < sensors.wall) // no west wall
    {
      motors.fwd(0, KP2 * (float)(sensors.east - sensors.side)); // follow east wall
    }
    else
    {
      motors.fwd(0, 0); // just go forwards
    }
  }
}

// get side width, north and wall limits
void calibrate()
{
  sensors.init(); // read sensors
  sensors.side = (sensors.east + sensors.west)/2; // set side limit

  motors.turn(E, 90); // turn east

  sensors.init(); // re-read sensors
  sensors.front = sensors.north; // set north limit
  sensors.wall = (sensors.side + sensors.west) / 2; // detect wall limit

  motors.turn(W, 90); // turn back west

  sensors.init(); // initialise sensors. west, north, east
}

void error()
{
  while(true)
  {
    digitalWrite(LED, HIGH);
    delay(100);
    digitalWrite(LED, LOW);
    delay(100);
  }
}

// DEBUG, print sensors data
void printsens()
{

  Serial.print("\n front wall    side   W      N      E      event\n");
  Serial.print(sensors.front);
  Serial.print("\t");
  Serial.print(sensors.wall);
  Serial.print("\t");
  Serial.print(sensors.side);
  Serial.print("\t");
  Serial.print(sensors.west);
  Serial.print("\t");
  Serial.print(sensors.north);
  Serial.print("\t");
  Serial.print(sensors.east);
  Serial.print("\t");
  Serial.println(sensors.state());
}

MOUSEMOTORS

// *** MouseMotors MASTER FILE *** 16-7-14

#include 
#include 

// servo pins west & east
#define WS 10
#define ES 11

// servo HALT position, forward SPEED speed, TURN speed, ROTATION (ms/deg) 
// and CM (ms/cm)
#define HALT 1500
#define SPEED 30
#define TURN 30
#define ROTATE 13
#define CM 185

// sensor bits, event value
//   3 2 1 0
// N 0 0 0 1 = 1
// W 0 0 1 0 = 2
// E 0 1 0 0 = 4
// S 1 0 0 0 = 8
// events map
//     7
//  3  1  5
// 2       4
#define N  0 
#define W  1
#define E  2
#define S  3

Servo westServo; // left side
Servo eastServo; // right side

class MouseMotors
{

public:
  // attach servos to pins
  void attach()
  {
    westServo.attach(WS);
    eastServo.attach(ES);
  }

  // halts the motors
  void halt()
  {
    westServo.writeMicroseconds(HALT);
    eastServo.writeMicroseconds(HALT);
    delay(100); // wait 100 ms for inertia
  }

  // forward for dist (cm, 0 = continuous) error + W, error - E
  void fwd(byte dist, float error)
  {
    westServo.writeMicroseconds(HALT + SPEED - error);
    eastServo.writeMicroseconds(HALT - SPEED - error);
    if(dist != 0)
    {
      delay(CM * dist);
      halt();
    }
  }

  // turn in direction by degrees
  void turn(byte dir, byte deg) // turn in dir by deg
  {    
    switch (dir)
    {
    case W: // left
      westServo.writeMicroseconds(HALT - TURN);
      eastServo.writeMicroseconds(HALT - TURN);
      break;
    case E : // right
      westServo.writeMicroseconds(HALT + TURN);
      eastServo.writeMicroseconds(HALT + TURN);
      break;
    }
    delay(deg * ROTATE); // convert degrees to ms runtime
    halt();
  } 
};

MOUSESENSORS

// *** MouseSensors MASTER FILE version for mouselabrynth *** 16-7-14
// cellwalls and mouse direction use NWES, see bitmap below and event values

#include 

// sensor emitter and detector L, F & R pins
#define LE 2
#define LD A0
#define FE 3
#define FD A1
#define RE 4
#define RD A2

// wall ID, event
// N 0 0 0 1 = 1
// W 0 0 1 0 = 2
// E 0 1 0 0 = 4
// S 1 0 0 0 = 8
// events map
//     7
//  3  1  5
// 2   6   4

// wall ID bits
#define N  0 
#define W  1
#define E  2
#define S  3

// number of sensor reading to take for init
#define AVG 10

class MouseSensors
{
public:
  // sensor smoothed readings
  int west;
  int north;
  int east;

  // north wall detect, side distance & side walls detect
  int front;
  int side;
  int wall;

  // init ports
  void enable()
  {
    pinMode(LE, OUTPUT);
    pinMode(FE, OUTPUT);
    pinMode(RE, OUTPUT);
  }

  // read sensors, laod west, north & east ints
  void sense()
  {
    float comb;
    float amb;
    float refl;

    // WEST
    digitalWrite(LE, HIGH); // emitter on
    delay(1);
    comb = analogRead(LD); // save combined value
    digitalWrite(LE, LOW); // emitter off
    delay(1);
    amb = analogRead(LD); // save ambient value
    refl = comb - amb;// calculate reflected

    west = int(refl * 0.1 + west * 0.9);

    // NORTH
    digitalWrite(FE, HIGH); // emitter on
    delay(1);
    comb = analogRead(FD); // save combined value
    digitalWrite(FE, LOW); // emitter off
    delay(1);
    amb = analogRead(FD); // save ambient value
    refl = comb - amb;// calculate reflected

    north = int(refl * 0.1 + north * 0.9);

    // RIGHT
    digitalWrite(RE, HIGH); // emitter on
    delay(1);
    comb = analogRead(RD); // save combined value
    digitalWrite(RE, LOW); // emitter off
    delay(1);
    amb = analogRead(RD); // save ambient value
    refl = comb - amb;// calculate reflected    

    east = int(refl * 0.1 + east * 0.9);
  }

  // read sensors, return event byte (0 - 7)
  byte state()
  {
    byte event;
    byte detect;

    sense();

    event = 0; // init to zero

    if(north >= front) event +=1; // add 1
    if(west >= wall) event += 2;  // add 2
    if(east >= wall) event += 4;  // add 4

    return event;
  }

  // init sensors averages by calling sense() AVG times
  void init()
  {
    byte i;

    for(i = 0; i < AVG; i++)
      sense();
  }
};

Mouse in a box sketch

Here's the mousebox.ino sketch. This keeps the mouse in a box of walls, changing direction if it comes near a wall.

MOUSEBOX

/* *** mousebox skech *** 16-7-14
sketch to run mouse in a box without touching the sides
 makes random moves
 */
#include 
#include "MouseSensors.h"
#include "MouseMotors.h"


// LED & button pins
#define LED 13
#define BUTTON 12

MouseMotors motors; // create motors object
MouseSensors sensors; // pass pin numbers to sensors object

void setup()
{
  Serial.begin(9600); // start serial comms

  motors.attach(); // attach servos
  sensors.enable(); // enable emiiter ports

  pinMode(LED, OUTPUT); // LED output
  pinMode(BUTTON, INPUT_PULLUP); // input is from button pin to GND

  // *** START
  while(digitalRead(BUTTON)); // wait for button press
  delay(100);
  while(!digitalRead(BUTTON)); // wait for button release
  digitalWrite(LED, HIGH); // LED on is a go
}

void loop()
{ 
  // avoid walls of box
  avoid(sensors.state());
}

void avoid(byte event) // take avoiding action depending on sensor event
{
  switch(event)
  {
  case 1: // north
    motors.turn(W, 90);
    sensors.init();
    break;
  case 2:  // west & north west
  case 3:
    motors.turn(E, 45);
    sensors.init();
    break;
  case 4: // east and north east
  case 5:
    motors.turn(W, 45);
    sensors.init();
    break;
  case 6: // east & west
  case 7: // east, north & west
    motors.turn(E, 180);
    sensors.init(); 
  default:
    motors.fwd(0,0); // forwards, continuous, no error correction
  }
}

Mouse Robot

Latest update 7-7-13

My Mouse Robot uses two continuous rotation servos and three IR emitter detectors pointing left, front and right. Acknowledgements to M Backus for the ideas and code principles.

The code that I have listed below are the classes, this will be followed in future posts by the application sketches.

Mouse sensors There are a number of IR emitters and detectors on the market, but the one I have had most success with is the TCRT5000 from Vishay.



This is a module with an IR emitter and IR phototransistor built in one package. The general wiring diagram for the eventual three detectors is:



The motors on the mouse are continuous rotation servos from Parallax.

LIBRARIES

There are two class files, for motors and sensors.

MOTORS

The calls to the motors class are:

motors.attach(); // connects the servo motors to the pins

motors.halt(); // stoops the motors

motors.fwd(dist, error); // motors forwards for diet (0 = continuous) with steering error

motors.turn(dir, deg); // turn West (left) or East (right) by degrees

// *** MouseMotors MASTER FILE *** 16-7-14

#include 
#include 

// servo pins
#define LS 10
#define RS 11

// servo HALT position, forward SPEED speed, TURN speed, ROTATION (ms/deg) 
// and CM (ms/cm)
#define HALT 1500
#define SPEED 30
#define TURN 30
#define ROTATE 13
#define CM 185

// proportional multiplier (float)
#define KP 0.5

// sensor bits, event value
//   3 2 1 0
// N 0 0 0 1 = 1
// W 0 0 1 0 = 2
// E 0 1 0 0 = 4
// S 1 0 0 0 = 8
// events map
//     7
//  3  1  5
// 2       4
#define N  0 
#define W  1
#define E  2
#define S  3

Servo leftServo;
Servo rightServo;

class MouseMotors
{

public:
  // attach servos to pins
  void attach()
  {
    leftServo.attach(LS);
    rightServo.attach(RS);
  }

  // halts the motors
  void halt()
  {
    leftServo.writeMicroseconds(HALT);
    rightServo.writeMicroseconds(HALT);
    delay(100); // wait 100 ms for inertia
  }

  // forward for dist (cm, 0 = continuous) error + W, error - E
  void fwd(byte dist, float error)
  {
    error = error * KP;
    leftServo.writeMicroseconds(HALT + SPEED -  error);
    rightServo.writeMicroseconds(HALT - SPEED -  error);
    if(dist != 0)
    {
      delay(CM * dist);
      halt();
    }
  }

  // turn in direction by degrees
  void turn(byte dir, byte deg) // turn in dir by deg
  {    
    switch (dir)
    {
    case W: // left
      leftServo.writeMicroseconds(HALT - TURN);
      rightServo.writeMicroseconds(HALT - TURN);
      break;
    case E : // right
      leftServo.writeMicroseconds(HALT + TURN);
      rightServo.writeMicroseconds(HALT + TURN);
      break;
    }
    delay(deg * ROTATE); // convert degrees to ms runtime
    halt();
  } 
};

SENSORS

The calls to the sensors class are:

sensors.sense(); // sense all three sensors and load west, north & east variables

sensors.state(); // returns an event (0-7) showing walls around the sensors

sensors.init(); // calls sense AVG times

sensors.calibrate(); // measured west and east sensors, takes average and sets thset distance for wall following

// *** MouseSensors MASTER FILE *** 16-7-14
// cellwalls and mouse direction use NWES, see bitmap below and event values
// this version is for mousebox.ino and uses a fixed detect distance in state()

#include 

// sensor emitter and detector L, F & R pins
#define LE 2
#define LD A0
#define FE 3
#define FD A1
#define RE 4
#define RD A2

// bitmap, event value
//   3 2 1 0
// N 0 0 0 1 = 1
// W 0 0 1 0 = 2
// E 0 1 0 0 = 4
// S 1 0 0 0 = 8
// resulting events map
//     7
//  3  1  5
// 2   6   4

#define N  0 
#define W  1
#define E  2
#define S  3

// number of sensor reading to take for init, event detect level
#define AVG 10
#define DETECT 20

class MouseSensors
{
public:
  // sensor smoothed readings
  int west;
  int north;
  int east;

  // threasholds for state() north detect and hall navigation
  int ndet;
  int hall;
  int wall;

  // init ports
  void enable()
  {
    pinMode(LE, OUTPUT);
    pinMode(FE, OUTPUT);
    pinMode(RE, OUTPUT);
  }

  // read sensors, laod west, north & east ints
  void sense()
  {
    float comb;
    float amb;
    float refl;

    // WEST
    digitalWrite(LE, HIGH); // emitter on
    delay(1);
    comb = analogRead(LD); // save combined value
    digitalWrite(LE, LOW); // emitter off
    delay(1);
    amb = analogRead(LD); // save ambient value
    refl = comb - amb;// calculate reflected

    west = int(refl * 0.1 + west * 0.9);

    // NORTH
    digitalWrite(FE, HIGH); // emitter on
    delay(1);
    comb = analogRead(FD); // save combined value
    digitalWrite(FE, LOW); // emitter off
    delay(1);
    amb = analogRead(FD); // save ambient value
    refl = comb - amb;// calculate reflected

    north = int(refl * 0.1 + north * 0.9);

    // RIGHT
    digitalWrite(RE, HIGH); // emitter on
    delay(1);
    comb = analogRead(RD); // save combined value
    digitalWrite(RE, LOW); // emitter off
    delay(1);
    amb = analogRead(RD); // save ambient value
    refl = comb - amb;// calculate reflected    

    east = int(refl * 0.1 + east * 0.9);
  }

  // read sensors, return event byte
  byte state()
  {
    byte event;
    byte detect;
    
    sense();
    
    detect = DETECT; // set detection limit
    event = 0; // init to zero

    if(north >= detect) bitSet(event, N); // 0001
    if(west >= detect) bitSet(event, W); // 0010
    if(east >= detect) bitSet(event, E); // 0100

    return event;
  }

  // init sensors averages by calling sense() AVG times
  void init()
  {
    byte i;

    for(i = 0; i < AVG; i++)
      sense();
  }
};