Wednesday 16 July 2014

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().

Screen Shot 2014 07 16 at 17 14 05

Here's my mouse in a simple labyrinth:

Photo 16 07 2014 16 59 06  HDR

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

Friday 4 July 2014

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.

Screen Shot 2014 07 04 at 10 58 21

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:

Screen Shot 2014 07 04 at 10 35 41

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