MyRaspberryAndMe

Tinkering with Raspberry (and other things)

Pi-Hicle final – motor-control and autonomous driving

So this is going to be the final part of the “Pi-Hicle” series (here are Part 1, Part 2, Part 3, Part 4). There is some good news and some bad news… But first a video of the vehicle moving:

The good news is that the BigTrak is in fact runnning on its own, avoiding obstacles with its three IR sensors. The bad news is that I have discarded the Raspberry Pi for this project. The vehicle is controlled by an Arduino mini now and there won’t be a Raspberry Pi in it in the near future. Now why this?

  1. I fell in love with the BigTrak and I simply can’t make any more holes in it, let alone ripping the keyboard off
  2. My plan to decrease the speed gradually as obstacles are detected does not work. There is not enough torque to move the wheels when the speed goes below 60% (and that is still too fast indoors, at least at my home)
  3. With the vehicle moving that fast a video camera is obsolete, one wouldn’t get a clear picture of anything (and I don’t have pets to annoy…)

So I am going to share the last steps in making this project. This involves mounting the sensors and putting everything together and the simple, yet working, code for making the BigTrak drive.

In part 4 I had investigated on the sensor accuracy. To use the measured values with my project I needed the inverse of that graph: the distance as a function of the reported signal d=f(s). I made numerous measurements and finally decided that a simple averaging was accurate enough. Take a look at the following graph:

distance_function

Not every measured point is plotted in the sheet, but one gets the idea. The usable distance range for me was from 100 cm to 30 cm, so I decided on implementing the function derived from the red curve. This would report lower distances and thus giving some added safety for the vehicle.

Assembly

Mounting the IR-sensors would have been easier with a Dremel tool. As I don’t have one I used an ordinary miniature drill and some sharp knives to cut holes in the BigTrak housing. Because I needed to insert the sensors at an angle I had to make the mounting holes a little bigger:

mounting_holes

The sensors were then positioned and fixed with a tiny spot of superglue and then finally mounted and secured with a hot glue gun. And it looks extremely like those great science fiction models from the 70’s and 80’s.

sensors_attached

Electronics

As I said before, I am using an Arduino mini (in fact I’m using a “Wattuino pro mini 5V” from Watterott) and a Polulu DRV8833 (Datasheet) motor shield for driving the motors. This is a simple H-bridge that can supply enough current for the motors of the BigTrak. The motors are powered by the batteries that would power the BigTrak anyways, in fact these batteries are able to supply the Arduino with power, too.
I intended to use an USB powerpack for powering the vehicle (and I ordered a 9000 mAh one) but had to realize that the powerpack does not deliver enough power to drive the motors (even on its 2A output). I am still investigating that…

The schematic is extremely simple. The three IR sensors go to Arduino analog pins, 4 digital (PWM) outputs connect to the H-bridge inputs; the H-bridge outputs run to the motors. The motor driver and the Arduino need a common ground signal, the BigTrak batteries are powering the motor shield and an external USB powerpack is powering the Arduino and the sensors. Here’s a simplified version:

schematic

(I copied the Arduino from the Fritzing libraries because I seem not to understand how to add new parts. It’s faster to scan the missing part, fire up Gimp and draw some paths…)
Here is a close-up of the circuit:

board_macro

All the parts fit on a small breadboard so I may reuse the Arduino in other projects. Of course I had to remove the original BigTrak board. It looks chaotic inside, but it works:

assembly_big

The “Logic”

The Arduino sketch is simple. The distances are continuosly monitored and the direction where there is the most “free space” is calculated. Then the command for moving in that direction is issued (if the direction has changed). In case all three sensors report obstacles the stop-command is called.

//
//    bigtrak_autonom.ino
//        Running a BigTrak toy car autonomously
//        with three IR sensors for distance measurement
//
//    Copyright (C) 2013  Thomas Henkel (mypiandme@gmail.com)
//
//    This program is free software: you can redistribute it and/or modify
//    it under the terms of the GNU General Public License as published by
//    the Free Software Foundation, either version 3 of the License, or
//    any later version.
//
//    This program is distributed in the hope that it will be useful,
//    but WITHOUT ANY WARRANTY; without even the implied warranty of
//    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//    GNU General Public License for more details.
//
//    You should have received a copy of the GNU General Public License
//    along with this program.  If not, see <http://www.gnu.org/licenses/>.
//

// pins for motor h-bridge
#define RMINUS 3
#define RPLUS  5
#define LMINUS 6
#define LPLUS  9

// pre-defined speed values
#define FULL  255
#define THIRD 160
#define HALF  90

// needed for direction case-switch
#define STOP 0
#define LEFT 1
#define FORWARD 2
#define RIGHT 3

// sensor phalanx inputs
int sensorRight = A0;
int sensorFront = A1;
int sensorLeft = A2;

// and threshold values for distance signal
int highThreshold = 400;
int lowThreshold = 150;
int minDistance = 30;

int steps = 100;              // measurements per sensor
int theSpeed = THIRD;         // current speed
int currentDirection = STOP;  // current direction

void setup()
{
  // define outputs
  pinMode(RMINUS, OUTPUT);
  pinMode(RPLUS, OUTPUT);
  pinMode(LMINUS, OUTPUT);
  pinMode(LPLUS, OUTPUT);

  // set reference for analog in
  analogReference(DEFAULT);
  // stop for start
    digitalWrite(RMINUS, LOW);
    digitalWrite(RPLUS, LOW);
    digitalWrite(LMINUS, LOW);
    digitalWrite(LPLUS, LOW);
    Serial.begin(115200);         // start serial for output

}

void loop()
{
  moveLoop();
}

boolean isDirectionChanged(int dir)
{
  if(currentDirection != dir)
  {
    currentDirection = dir;
    return true;
  }
  else return false;
}

/**
 ** get the direction from the sensor measurements
 ** if the direction has changed call new direction
**/
void moveLoop()
{
  int dir = calcDirection();
  switch(dir)
  {
    case STOP:
      if(isDirectionChanged(dir)) halt();
      break;
    case LEFT:
      if(isDirectionChanged(dir)) left(HALF);
      break;
    case FORWARD:
      if(isDirectionChanged(dir)) forward(theSpeed);
      break;
    case RIGHT:
      if(isDirectionChanged(dir)) right(HALF);
      break;
    default:
      halt();  // just in case this gets hit
  }
}

/**
 ** calculate the distance from one sensor
 ** average over "cycles" measurements
**/
int getDistance(int pin, int cycles)
{
  int distance = 999;
  unsigned long signal = 0;
  for(int i=0;i<cycles;i++)
  {
    signal+= analogRead(pin);
  }
  signal = signal/cycles;
  if(signal<lowThreshold)
  {
    return 999;
  }
  if(signal>highThreshold)
  {
    return 10;
  }
  distance = (int)(-47.4*log(signal)+312);
  return distance;
}

/**
 ** calculate the direction from the measured
 ** distances of all three sensors
**/
int calcDirection(void)
{
  int left  = getDistance(sensorLeft, steps);
  int front = getDistance(sensorFront, steps);
  int right = getDistance(sensorRight, steps);
  // check if forward is OK
  if(front >  minDistance)
  {
    // check if we bump into things on side
    if(left<minDistance) return RIGHT;
    if(right<minDistance) return LEFT;
    // no.ok, go.
    return FORWARD;
  }
  if(left<minDistance && right<minDistance && front<minDistance)
  {
    return STOP;
  }
  if(left>right)
  {
    return LEFT;
  }
  else
  {
    return RIGHT;
  }
}

/**
 ** set pins for h-bridge according to command
**/
void setPins(int command)
{
  Serial.print ("Setting: ");
  Serial.println(command);
  digitalWrite(LPLUS, command&0x01);
  digitalWrite(LMINUS, command&0x02);
  digitalWrite(RPLUS, command&0x04);
  digitalWrite(RMINUS, command&0x08);
}
void setPins(int command, int iSpeed)
{

  Serial.print ("Setting: ");
  Serial.println(command);
  analogWrite(LPLUS, iSpeed*bitRead(command,0));
  analogWrite(LMINUS, iSpeed*bitRead(command,1));
  analogWrite(RPLUS, iSpeed*bitRead(command,2));
  analogWrite(RMINUS, iSpeed*bitRead(command,3));
}

/**
 ** functions for direction commands
**/
void forward(int s)
{
  setPins(0b1010, s);
}
void back(int s)
{
  setPins(0b0101, s);
}
void right(int s)
{
  setPins(0b0110,s);
}
void left(int s)
{
  setPins(0b1001, s);
}
void halt()
{
  setPins(0b0000);
}

The finished BigTrak

final_front

What else?

Well, that’s it for now. As a proof of concept a toy vehicle can be run autonomously (sort of) with very few parts and an extremely simple logic. I am thinking about trying to re-enable the original PCB, so that I may switch between “autonomous” and “original” mode. Adding a Raspberry Pi or a webcam is no longer an option. I will not further modify the BigTrak’s chassis and housing and there are literally hundreds of projects out there that have done that before.

Perhaps sometime I will get myself a real roboter chassis and rebuild a vehicle from scratch that will be wirelessly controlled by a Raspberry Pi. Or I am going to try to remotely control the Arduino in my vehicle via Bluetooth from a Raspberry Pi. The framework from my LED sign would be a good start.

Advertisements

Comments are closed.