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

Fun little project at GB5BRA Banbury Show

The Banbury Amateur Radio Society will be at the annual Banbury show on Sunday 8th June. We will be operating a station GB5BRA and look forward to contacting you...

Also at the show we will be promoting amateur radio. For this we have a display panel "LEARN _ BUILD _ PLAY". And a tiny demo using an Arduino to send an automated morse message "CQ CQ CQ DE GB5BRA GB5BRA K".

Circuit

Here's the code:

// Morse transmitter, with oscillator 
// LED & OSC Pin 7

// output pin
#define LEDPIN 7

// text array codes
#define DOT 0
#define DASH 1
#define SPACE 2
#define WORD 3

// message words
int CQ[] = {1,0,1,0,2,1,1,0,1,3};
int DE[] = {1,0,0,2,0,3};
int GB5BRA[] = {1,1,0,2,1,0,0,0,2,0,0,0,0,0,2,1,0,0,0,2,0,1,0,2,0,1,3};
int K[] = {1,0,1,3};

// dash = 3 dots, space = 3 dots, word = 7 dots
#define DOTTIME 150

void setup() 
{
  pinMode(LEDPIN, OUTPUT);
}

void loop() 
{

// send message words, sends CQ CQ CQ DE GB5BRA GB5BRA K
  sender(CQ);
  sender(CQ);
  sender(CQ);
  sender(DE);
  sender(GB5BRA);
  sender(GB5BRA);
  sender(K);
  
  delay(2000); // repeat after 2 secs
}

// send a word
void sender(int dida[]) {

  int x = 0; // array index

// send characters up to end of word
  while(dida[x] != WORD) {
   switch (dida[x]) {
    case DOT:
       transmit(DOTTIME);
       delay(DOTTIME); // add after dot
       break;
    case DASH:
        transmit(3 * DOTTIME);
        delay(DOTTIME); // add after dash
        break;
    case SPACE:
         delay(2 * DOTTIME);  // add for space
         break;
    }
    x++;
  }
  delay(6 * DOTTIME); // add   for word
}

// transmit for time (msec)
void transmit(long time) {

  digitalWrite(LEDPIN, HIGH); // turn on LED
  delay(time); // time 
  digitalWrite(LEDPIN, LOW); // turn off LED
}

Update: Its a pity but the Banbury Show, where the project would be shown, was cancelled 3 days before it was due, because of a water logged field.

SI514 arrived from Silicon Labs

A big parcel was delivered yesterday.



Inside was a smaller parcel.



And inside that a small box.



Containing two very small Si514 components!!!



Thank you Silicon Labs for these free samples.

Progress in SDR RX

So here's an update on the design of an Arduino Shield SDR RX.

First the AD9850 is no good, as it has not got 50:50 ratio output square waves. I need 50:50 to make my clock generator, which only divides by /2 not /4 of other designs - thus he VFO is half the frequency... and the Si570 is no good as it does not have complementary outputs. But the Si514 does, and this seems a good choice.





This is my circuit. The 7474 flip-flops and driven by complementary clock inputs and give quadrature output for the SDR switches (FST3253).


The complete circuit is this:

AD9580 DDS for SDR

I think I have invented something new!!! What I am trying to do is to build an SDR on an Arduino shield, for this I thought of using a DDS frequency synthesiser and an FT3253 MOS switch - in a design similar to a lot found on the web.

The idea is to drive the AD9850 chip module from the Arduino and display the band and frequency on an LCD screen. (See previous posting). This all works fine.

The new idea was to take the square wave outputs from the AD9580 module and drive a couple of 7474 dividers, like this



This is different from the usual circuit which drives the two clock inputs together, but which divides the input frequency by 4, here we divide by 2. This means that at the upper frequency of the AD9850 module of 40MHz we can tune our radio up to over 20MHz, so covering 160-20m bands.

I don't have an oscilloscope, but I have discovered and bought ($30) a small module called the Logic Pirate, and together with some software called LogicSniffer, I have had a look at the waveforms. They are like this:



Here you can see that the basic logic is OK, the input waveforms (CH 0) have rising edges which trigger the flip-flops twice per cycle, and produce quadrature outputs (CH 1 & 2) . But there is a BIG problem, the quad outputs are not equal lengths. In theory you can adjust the mark/space ratio of the AD9850 module square wave outputs with a small pot on the board, but I have found that this is very, very sensitive, and at no position can I get equal times.

So that puts this design out, as the quad signals MUST have equal times for the Tayloe detector to work correctly.

Si570??

Now to start to think of a solution using the Si570 synthesiser chip. But for this I need a PCB and will recreate it in Eagle software to generate a small module with 0.1" pinouts. This can then be used on an Arduino Shield together with the module I have of the FT3253 on a similar 0.1" pitch. I will need to provide 3.3V for the Si570, and level shift the input SDA & SCL lines from the 5V signals of the Arduino... more later as I get this done. Here's the circuit: