Colin’s Spooky Brain Transplant!

Posted on November 20, 2016 by 1988kramer

In the spirit of Halloween (which I realize was nearly a month ago), I’ve done some unholy, Frankenstein-esque modifications to Colin’s brain!

Moving the Arduino to the Breadboard

First of all, I’ve been using an Arduino as Colin’s brain but an Arduino is actually pretty bulky, considering its capabilities. So I’ve replaced this:

with this:

It’s an ATmega328; the same chip the Arduino uses. If you flash it with an Arduino bootloader you can program and use it exactly like you would an Arduino. This page has a great tutorial for programming and using the bare chip on a breadboard. Also, if you’re willing to pay a little extra you can buy a chip that has the bootloader pre-installed. Both SparkFun and Adafruit sell pre-programmed ATmega328s.

To switch from an Arduino to an ATmega328 I just needed to rejigger my power supply a bit and rewire everything for the new pin mapping (good thing I kept detailed notes on my wiring, right?)

Next, Colin’s control libraries are getting pretty big. Right now they take up about 25% of the ATmega’s memory. When all’s said and done, the control libraries should only be a small part of his program’s total size. So we’re going to need some more space before long.

Also, writing and testing code with the Arduino is getting to be kind of a pain. First, you have to upload your program with the Arduino IDE every time you make a change or want to run a new program. This can get pretty cumbersome, especially if you want to make minor tweaks to a program or try different versions of it. Also, there’s no possibility for multi-threading or concurrent execution. So you’re pretty limited in what your programs can do.

Adding Computing Power!

The solution to this could be to use a more powerful single-board computer like a Raspberry Pi. Since a Pi just runs linux, I can run write, edit, and compile programs on the Pi itself. Also, because it has WiFi, I can leave it connected to Colin and control it via SSH. This makes programming much, much easier. However, it has a couple of significant downsides. It only has one PWM pin, so it can’t directly control two motors. Also, it can’t handle hardware interrupts so it can’t really use motor encoders either.

My solution is to keep the ATmega for low-level motor and sensor control while the Raspberry Pi handles the high-level processing. The Pi and the ATmega will communicate over a serial bus. They will use this to relay commands, position updates, sensor readings, and so on.

This means Colin now has two brains! This is what he looks like with his new braaaains!

Note that the Raspberry Pi uses 3.3V logic and all of the other hardware runs at 5V, so I need to use a level shifter to communicate between the Pi and the ATmega. Also, the Raspberry Pi runs on 5V USB power. I already have my voltage regulator supplying 5V for my ATmega, so I simply cut the end off an old micro USB cable and connected the +5V and GND wires to my breadboard’s power rail. The Pi draws a lot of current, around 1.3A under stress, and my voltage regulator can only provide 1.5 to 1.75A. So I might need to upgrade to bigger voltage regulator soon.

I’m also re-configuring Colin’s sonar sensors. I will start out with 8 sensors arranged in a ring, controlled independently by an ATtiny84. Since each sensor has a range of about 15 degrees, 24 total sensors are necessary to completely cover 360 degrees. I’m not sure I’ll need to actually use 24 sensors, but my design will allow me to add additional sensor rings if necessary. I’ll go into detail on these in a later post.

I’ll explain how to coordinate the raspberry pi and the ATmega328 using serial communication in a later post, and I’ll provide an overview of my ATtiny-based sensor controllers shortly thereafter!

Odometry With Arduino

Posted on November 19, 2016 by 1988kramer

Now that we can control the speed of Colin’s wheels, we can tell how far Colin has moved using odometry. It involves counting the encoder ticks for Colin’s motors and integrating that information over time to determine Colin’s change in position. This method has the distinct advantage that it relies on the actual motion of Colin’s wheels, and thus doesn’t require absolute accuracy from the speed control algorithm. Odometry also provides a good motion model that can be used as part of a larger localization algorithm. As such it’s a good stepping stone toward my goal of making a simultaneous localization and mapping program for Colin!

This tutorial owes a lot to MIT’s primer on odometry and motor control. It does a great job explaining the theory behind odometry.

Theory Basics

The position of a robot in space is referred to as its pose, which is defined by six quantities: its translation in Cartesian coordinates (x, y, and z) and its rotation about those three axes (θ_x, θ_y, and θ_z). Luckily, a differential drive robot like Colin can only translate in two dimensions and rotate in one, so Colin’s pose can be defined by three quantities (x, y, and θ_z).

Let’s say Colin’s initial pose is (0, 0, 0) at $t=t_{0}$ . How can we determine his change in pose when $t=t_{0}+\Delta t$ where $\Delta t$ is the time interval between pose updates? Because we’re already using encoders to control Colin’s speed, it’s easy to keep track of the distance Colin’s wheels have turned. In fact, my Encoder class already does this with its getDistance() function.

Let’s say $d_{left}$ is the distance turned by the left wheel over $\Delta t$ , and $d_{right}$ is the same quantity for the right wheel. Knowing these two distances can tell us a couple things. If $d_{left}=d_{right}$ then Colin traveled in a straight line. If $d_{left}\gt d_{right}$ he turned to the right and if $d_{left}\lt d_{right}$ he turned left. We can also use $d_{left}$ and $d_{right}$ to calculate Colin’s exact translation and rotation.

To simplify things a bit we’ll assume Colin’s wheel speeds are constant, which adds a negligible amount of error as long as we keep $\Delta t$ small. This assumption means that Colin is always travelling along a circular arc. The length of this arc, $d_{center}$ is given by the average of $d_{left}$ and $d_{right}$ :

$d_{center}=\frac{d_{left}+d_{right}}{2}$

We’ll say that Colin’s rotation in radians over $\Delta t$ is $\phi$ . Also, let $r_{left}$ be the distance between the center of Colin’s arc of travel and his left wheel and $r_{right}$ be the same distance for the right wheel. This means that $d_{left}=\phi r_{left}$ and $d_{right}=\phi r_{right}$ . Also, $r_{left}=r_{right}+d_{wheels}$ where $d_{wheels}$ is the distance between Colin’s wheels. With a little bit of algebra we can show the following:

$\phi=\frac{d_{right}-d_{left}}{{d_{wheels}}}$

We can also calculate Colin’s change in his x and y coordinates via the following equations:

$x'=x+d_{center}cos(\theta)$

$y'=y+d_{center}sin(\theta)$

Where $x'$ and $y'$ are the new x and y position, respectively. It’s important to note that the above equations are simplified. They assume that Colin’s motion happens in two discrete phases: he rotates in place and then translates along a straight line. This is clearly not true, but as long as $\phi$ is small, the error introduced is negligible. This means that, as with our prior simplification, we need to keep $\Delta t$ small to make this work. I’m not going to go into all the details here, but if you’re interested you can find the full derivation in the MIT odometry tutorial.

So, now that we have worked out the mathematical underpinnings for odometry, we can translate this into code!

Odometry Code

The magic happens in my new DifferentialDrive library. We’ll just go over the odometry portion today, but DifferentialDrive allows the user to control an arbitrary differential drive robot by specifying the robot’s translational and angular velocities and, optionally, the distance the robot should travel. I’ll explain all of that in a later post and include some implementation examples as well!

void DifferentialDrive::updatePosition()
{
   // get the angular distance traveled by each wheel since the last update
   double leftDegrees = _leftWheel->getDistance();
   double rightDegrees = _rightWheel->getDistance();

   // convert the angular distances to linear distances
   double dLeft = leftDegrees / _degreesPerMillimeter;
   double dRight = rightDegrees / _degreesPerMillimeter;

   // calculate the length of the arc traveled by Colin
   double dCenter = (dLeft + dRight) / 2.0;

   // calculate Colin's change in angle
   double phi = (dRight - dLeft) / (double)_wheelDistance;
   // add the change in angle to the previous angle
   _theta += phi;
   // constrain _theta to the range 0 to 2 pi
   if (_theta > 2.0 * pi) _theta -= 2.0 * pi;
   if (_theta < 0.0) _theta += 2.0 * pi;

   // update Colin's x and y coordinates
   _xPosition += dCenter * cos(_theta);
   _yPosition += dCenter * sin(_theta);
}

The above function needs to be called every $\Delta t$ and, to keep the error from our simplifications small $\Delta t$ needs to be small. In my testing I’ve found that doing a position update with the same frequency as the updates for the PID motor controller (every 50ms) results in good accuracy over short distances. However, this update involves a significant amount of extra computation, and doing it 20 times per second might require an excessive amount of processor time if you’re trying to do a lot of other computation at the same time. I’ve found that doing position updates half as often (every 100ms) results in very little loss of accuracy, so it’s entirely possible to balance accuracy and the resources your program has to spare.

Further Work

First of all, we need to integrate the above update function into the larger class that controls Colin’s motion. I’ll demonstrate that in a later post and include some examples that show how to use the class in an Arduino sketch.

Also, odometry can only be used to calculate Colin’s position relative to his starting position. It cannot be used to determine his absolute position in a space unless his starting position is known.

The larger problem is that odometry is inherently inaccurate. Encoder ticks do not translate directly into distance traveled by the wheel because wheels slip, the wheels aren’t perfectly circular, the ground isn’t perfectly flat, encoder ticks might be missed, and the motor gearbox has backlash that isn’t accounted for in our model. This means that Colin’s position calculated from odometry will gradually diverge from his true position. We could use other methods that might be more accurate, such as optical flow and IMUs. However, any sensor we might use suffers from some inherent random error, known as noise, and this error will accumulate over time.

To compensate for this error we can calculate Colin’s probable position by incorporating data from another sensor. This is what I’ll be working on over the next several months. First I’ll develop a program to localize him to a pre-existing (or a priori) map, and then I’ll work on a program that allows him to build his map on the fly.

I should note that software for this purpose is already available as part of the robot operating system (ROS), but I’m not interested in pre-made solutions. My goal here is to develop these solutions myself so we can all learn the intimate details of their operation.

PID Motor Control

Posted on July 8, 2016 by 1988kramer

Months ago I wrote a post on the use of motor encoders with Arduino and promised to follow up with a post on speed control. In fact at the time I already had a mostly functional speed control class but I decided I had to do some major cleanup on it before presenting here. Then I started school I had no time for Colin or this blog for nine months.

Now, that I’m done with school I’ve had time to write a brand new motor control library! You can find the source code and an example sketch at github.com/1988kramer/motor_control.

In this post I’m going to briefly explain how my motor control program is structured, then give some background on the control theory that went into it, then I’ll explain how the theory is implemented in the actual code.

Program Structure

I structured my program to mirror the hardware as closely as possible so I made two classes, Motor and Encoder, to handle the low level functions of the motor and encoder. The Motor and Encoder classes are used by the SpeedControl class to precisely control the motor’s speed. See the diagram below for a detailed explanation of the the library’s structure.

GDE Error: Error retrieving file - if necessary turn off error checking (404:Not Found)

You’ll notice two classes in the diagram above that use the functionality of SpeedControl: PositionControl and DifferentialDrive. PositionControl uses an instance of SpeedControl to provide position control functions and DifferentialDrive uses two instances of PositionControl to provide high level control for a differential drive robot like Colin.

I’m just going to be discussing SpeedControl class today, but you can expect a post on PositionControl and DifferentialDrive in the next few weeks.

Control Loop Basics

In my earlier post on motor control I explained that simply using analogWrite does not actually set the speed of a motor. It only sets the voltage to the motor. Varying loads will cause a motor to run at different speeds with the same input voltage. How do we get a motor to run at a specific speed? This sounds like a job for a closed-loop controller!

In a closed-loop controller, we tell the controller how fast we want the motor to run. We call this the set point. The controller then measures the actual speed of the motor and calculates the difference between the actual speed and the set point. This is the error. The controller then adjusts the voltage to the motor to reduce the error.

Proportional Control

This sounds simple enough, but how much should the controller adjust for a given error? One could make the adjustment directly proportional to the error, i.e. large errors get large adjustments and small errors get small adjustments. The adjustment would be defined as

$A(t)=K_{{p}}e(t)$

where $A(t)$ is the adjustment, $e(t)$ is the error, and $K_{p}$ is a constant referred to as the gain. This is a decent solution but it has some problems. If $K_{p}$ is large the controller will be quick to respond to disturbances but it will tend to over-correct (known as overshooting). If $K_{p}$ is very large the motor speed will oscillate around the set point and never stabilize. If $K_{p}$ is small enough to avoid overshooting, however, the controller will respond very slowly to disturbances.

Integral and Derivative Terms

To compensate for this we can add a couple more terms to the adjustment equation. A term proportional to both the error and duration of the error, called the integral term, will correct for persistent small errors that won’t be corrected by the proportional term. It will also make the controller faster to reach the set point. However, if the integral term is too large it will cause overshoot and oscillation. The integral term is defined as

$I_{out}=K_{i}\int_{0}^{t}e(\tau)d\tau$

Lastly, the derivative (slope) of the error over time can be used to predict the system’s future behavior. For instance, a large derivative indicates the next error will likely be far away from the last whereas a small derivative indicates the next error will likely be close to the last. A term calculated from the derivative of past errors helps to minimize overshoot and improve the stability of the controller. The derivative term is defined as

$D_{out}=K_{d}\frac{de(t)}{dt}$

PID Adjustment Equation

So our final adjustment is the sum of the proportional, integral, and derivative terms:

$A(t)=P_{out}+I_{out}+D_{out}=K_{p}e(t)+K_{i}\int_{0}^{t}e(\tau)d\tau+K_{d}\frac{de(t)}{dt}$

Every time the controller adjusts the voltage output to the motor it must find the current speed of the motor, calculate the error between the current speed and the set point, then use the above equation to determine how much the output voltage to the motor should be adjusted to compensate for the error. In Colin’s case this is done 20 times per second.

The Code

Because posting my motor control code in its entirety would take far too much space and most of it isn’t very interesting anyway, I’m just going to post and explain the update function. It calculates the required adjustment to the PWM output to the Motor to maintain consistent speed.

My code draws heavily on Brett Beagregard’s PID library. I would encourage you to check out his blog for a more in-depth explanation of PID control. Thanks for your work, Brett!

// adjusts motor's PWM level to correct for any error between the
// set point and the actual speed of the motor's output shaft
// MUST be called regularly on the same deltaT used to calculate
// motor speed in the Encoder object
void SpeedControl::adjustPWM()
{
	int speed = _encoder->getSpeed(); // motor control returns vector speed
	if (speed < 0) speed *= -1;  // convert speed to scalar
	int error = _setPoint - speed;  // calculate error
	_iTerm += (_kI * (double)error); // calculate integral term
	double dInput = speed - _lastSpeed; // calculate derivative
	int adjustment = (_kP * (double)error) + _iTerm - (_kD * dInput);
	_pwm += adjustment;
	constrainPWM(); // limit _pwm to the range 0-255
	_motor->setPWM(_pwm);
	_lastSpeed = speed;
}

The code above basically performs the calculation discussed in the preceding section. In line 7 it fetches the current speed of the motor and calculates the error in line 9. It then calculates the proportional, integral, and derivative terms and uses them to calculate the adjustment in lines 10 through 12.

Derivative Kick

There is one deviation from the equations presented in the previous section: in lines 11 and 12 we’re using the speed, not the error to calculate the derivative term. Also, we’re subtracting it from the adjustment. What gives?

Basically, when the user changes the set point there is an instantaneous change in the error $e(t)$ . This means the derivative of the error $\frac{de(t)}{dt}$ is infinite at this point. So in theory the derivative term becomes infinite whenever the set point changes, a phenomenon called derivative kick. Because the code above calculates the approximate numerical derivative the phenomenon is not as pronounced, but it should still be addressed. Making the change above corrects for derivative kick. See Brett’s blog for a more in-depth explanation.

Example Sketch

So how do we actually implement this to control a motor? You could spend weeks or possibly months of your free time developing a full speed control program, or you could try using the one that I spent weeks of my free time on! The example sketch below demonstrates the use of my SpeedControl class, available on github.

#include <TimerOne.h>
#include <PositionControl.h>

const int dir1 = 9, dir 2 = 8, pwm = 6;
const int encoderA = 3, encoderB = 5;
const int deltaT = 50000;
const int encoderTicksPerRev = 309;
const double kP = 0.025, kI = 0.0008, kD = 0.0005;

Motor rhMotor(dir1, dir2, pwm);
Encoder rhEncoder(encoderA, encoderB, deltaT, encoderTicksPerRev);
SpeedControl rhSpeedControl(&rhMotor, &rhEncoder);

  
void setup() {
  Serial.begin(9600);
  Timer1.initialize(deltaT);
  Timer1.attachInterrupt(adjustPWM);
  attachInterrupt(1, readRHEncoder, CHANGE);
  rhSpeedControl.setGains(kP, kI, kD);
}

void loop() {
  
  rhSpeedControl.setSpeed(180);
  delay(5000);
  rhSpeedControl.setSpeed(0);
  Serial.println(rhSpeedControl.getDistance());
  delay(2000);
}

void readRHEncoder()
{
  rhEncoder.updateCount();
}

void adjustPWM()
{
  rhSpeedControl.adjustPWM();
}

Implementation Notes

A couple of notes on note on the sketch above:

First, the PID gains, kP, kI, and kD used in the example sketch work well for Colin but will need to be tuned for your robot. PID tuning is something of a black art; I haven’t been able to find much info on it online, anyway. The method presented on this wikipedia page worked fairly well for me though.

Second, the SpeedControl.adjustPWM() function must be called every deltaT microseconds using a timer interrupt. See this Arduino playground page for more info on timer interrupts. The deltaT value needs to be tuned just like the PID gains. A deltaT value that’s too large will result in the controller not adjusting frequently enough to accurately control the motor’s speed. A value that’s too small, will limit the accuracy of the speed measurement because fewer encoder ticks can elapse in a smaller amount of time. The maximum accuracy for a given deltaT, by the way, is

$Accuracy=\frac{D}{\Delta t}$

Where D is the number of degrees per encoder tick. I found a deltaT of 50 milliseconds works well for Colin, but you should play around with it for yourself.

That’s all I have for now! Stay tuned for a post on my PositionControl and DifferentialDrive classes in the next few weeks!

I Finally Learned Git!

Posted on April 9, 2016 by 1988kramer

If anyone’s tried to look at my GitHub page they may have noticed it didn’t contain any files and wasn’t really set up. I’m proud to announce that changed today! I finally learned to use git and I’ve made a new repo for the examples used on this blog! I’m not going to attempt an in-depth git tutorial, mostly because there’s a already a lot of great resources out there. I mostly used this one. I do have a bit of advice though: use the command line interface!

I tried to learn git using the graphical interface several months ago and I found it was really difficult to understand. In the intervening time I got a lot more comfortable with BASH and I recently tried learning git again using the command line interface. Seriously, use command line if you want to learn git. The command line makes it much easier to understand how git is structured and how it works.

tl;dr The link to my GitHub profile at the bottom of the page now has a purpose!

Motor Encoders with Arduino

Posted on January 1, 2016 by 1988kramer

If Colin is going to create a map of the space he’s in, he needs to be able to find the distance between objects in the space. If he’s going to accurately control his own motion, he needs to be able to control the speed of his motors. For both of these things he needs information about how far and how fast his motors are turning. To give Colin that information I added encoders to his motors. Encoders are special sensors that track both how far his motor shafts have turned, and in what direction.

The following tutorial will discuss how to use shaft encoders with DC motors. First I’ll discuss the principles the encoders work on, then I’ll show you how to wire up an encoder and verify that it’s working. This tutorial owes a large dept to the Penn State Robotics Club Wiki, which has an excellent page on wheel encoders.

For this tutorial I’ll be using these magnetic encoders from Pololu. They are great for use with an Arduino because their output is a nice, easy to read, digital square wave. Pololu also offers an analog optical encoder, but to reliably read its output you need to use a ADC (analog to digital converter). Also, the optical encoder is susceptible to interference from bright light. So magnetic encoders are easier to use and more reliable for our purposes.

Encoder Principles

Pololu’s shaft encoders add a 6 pole magnetic disc to the shaft of the motor, as well as two Hall effect sensors. As the motor turns the disc rotates past the sensors. Each time a magnetic pole passes a sensor, the encoder outputs a digital pulse, also called a “tick.” The encoder setup is pictured below.

motor encoder installation example (from pololu.com)

The encoder has two outputs, one for each Hall effect sensor. The sensors are separated by 90 degrees. This means the square wave outputs of the sensors are 90 degrees out of phase. This is called a quadrature output. The picture below (taken from Pololu’s website) shows the typical output of an encoder.

Output of an encoder (from pololu.com)

Why is it important that the output pulses are 90 degrees out of phase? This allows us to determine both the magnitude and the direction of the motor’s rotation. If output A is ahead of output B, then the motor is turning forward. If output A is behind B, the motor is turning backward. Pretty cool, right?

Encoder Setup and Wiring

Wiring up the encoders is pretty simple. See the diagram below for details on wiring up the encoder for motor B, and repeat for motor A.

wiring for a single motor and encoder

Note that the encoder pin OUTA needs to be connected to a hardware interrupt pin (digital pin 2 or 3 on an Arduino Duemilanove or Uno). For this example it’s not really necessary to use hardware interrupts but if you don’t your Arduino won’t be able to do anything but track the encoder. Interrupts allow your Arduino to keep track of the encoders while it’s running another program.

It’s also important when installing your motors to not mount them too close to each other. If your motors are mounted back-to-back, the magnetic encoder wheels will interfere with each other. I initially had only about 5mm between the two encoder wheels. The magnets in the encoder wheels linked with each other and made it impossible for the two motors to turn at different speeds. Keeping the encoder wheels at least 20mm apart seems to avoid any interference problems.

Hardware Interrupts

It’s pretty easy to write a program that simply counts the number of ticks. But if, for some reason, you want your program to do anything but track the encoders you need to use hardware interrupts.

With hardware interrupts your main program runs until the state of one of the Arduino’s interrupt pins changes. Then the main program is stopped, a special interrupt method is run, then the main program is resumed. For example, an obstacle avoidance routine can run in the foreground but if one of the interrupt pins changes from low to high the position or speed of the motor can be updated in the interrupt method.

Because the main program is stopped when the interrupt method is triggered, the length of the interrupt method should be kept as short as possible. If the interrupt method is too long, then the Arduino will spend all of its time running the interrupt method and it will never actually run the main program.

For more details on hardware interrupts check out this page on the Arduino playground.

Example Sketch

The sketch below simply prints the values of the two count variables. When the motors are moved, the encoder output triggers the appropriate encoder event method. The encoder event methods either increment or decrement the count variables, keeping track of the number of ticks from each encoder. If the motor is turned forward, the count is incremented and it is decremented if the motor is turned backward.

The motors need to be moved manually for this sketch. They won’t move themselves. We will expand this to do simple motor speed control in a later tutorial.

We’ll define motor direction somewhat arbitrarily: If RH_ENCODER_A is HIGH and RH_ENCODER_B is LOW then the motor is turning forward. If RH_ENCODER_A is HIGH and RH_ENCODER_B is also HIGH, then the motor is turning backward. You can swap the wires for ENCODER_A and ENCODER_B to change the direction if required.

/*
 * Encoder example sketch
 * by Andrew Kramer
 * 1/1/2016
 *
 * Records encoder ticks for each wheel
 * and prints the number of ticks for
 * each encoder every 500ms
 *
 */

// pins for the encoder inputs
#define RH_ENCODER_A 3 
#define RH_ENCODER_B 5
#define LH_ENCODER_A 2
#define LH_ENCODER_B 4

// variables to store the number of encoder pulses
// for each motor
volatile unsigned long leftCount = 0;
volatile unsigned long rightCount = 0;

void setup() {
  pinMode(LH_ENCODER_A, INPUT);
  pinMode(LH_ENCODER_B, INPUT);
  pinMode(RH_ENCODER_A, INPUT);
  pinMode(RH_ENCODER_B, INPUT);
  
  // initialize hardware interrupts
  attachInterrupt(0, leftEncoderEvent, CHANGE);
  attachInterrupt(1, rightEncoderEvent, CHANGE);
  
  Serial.begin(9600);
}

void loop() {
  Serial.print("Right Count: ");
  Serial.println(rightCount);
  Serial.print("Left Count: ");
  Serial.println(leftCount);
  Serial.println();
  delay(500);
}

// encoder event for the interrupt call
void leftEncoderEvent() {
  if (digitalRead(LH_ENCODER_A) == HIGH) {
    if (digitalRead(LH_ENCODER_B) == LOW) {
      leftCount++;
    } else {
      leftCount--;
    }
  } else {
    if (digitalRead(LH_ENCODER_B) == LOW) {
      leftCount--;
    } else {
      leftCount++;
    }
  }
}

// encoder event for the interrupt call
void rightEncoderEvent() {
  if (digitalRead(RH_ENCODER_A) == HIGH) {
    if (digitalRead(RH_ENCODER_B) == LOW) {
      rightCount++;
    } else {
      rightCount--;
    }
  } else {
    if (digitalRead(RH_ENCODER_B) == LOW) {
      rightCount--;
    } else {
      rightCount++;
    }
  }
}

That’s all for today. In my next post I’ll take what we learned about using encoders with hardware interrupts and incorporate that into a simple speed control routine. After that we can dive into real PID control!

Bitwise Obstacle Avoidance Tweak

Posted on August 30, 2015 by 1988kramer

Around the time I was rewriting the example sketch for the obstacle avoidance tutorial I stumbled across this great tutorial on bit math on the Arduino Playground. While reading it I found a good, simple use for bit math that would make my obstacle avoidance sketch a bit more efficient.

There is a popular quote, usually attributed to Don Knuth: “premature optimization is the root of all evil.” Premature optimization occurs when we make improvements to efficiency that complicate the program, reduce readability, or cause us to ignore larger, more structural improvement possibilities.

The following is definitely a premature optimization of the obstacle avoidance example sketch. However, I think it’s okay in this case because it allows us to practice a fun new technique: bit math!

Background

First thing’s first: if you haven’t read the Arduino Playground tutorial on bit math, read it now. It’s good, it’s easy to read, and it provides much more background than I’m going to.

In my obstacle avoidance example I used a multi dimensional array of bools to store the sensor states for each possible reaction, the reaction matrix. Each bool value occupies one byte, so the size of the reaction matrix in bytes is the number of sensors times the number of reactions. In our case it’s 24 bytes.

Each byte is made of 8 bits, each of which have two states. A bool value only has two states, so in theory a bool could be represented by a single bit. Further, a byte could be interpreted as an array of 8 bools. Cool, right? This means, if the number of sensors is 8 or less, the size of our reaction matrix in bytes is just the number of reactions. The size of our reaction matrix is only 8 bytes now!

Additionally, our compareCases() can be totally eliminated by this. It formerly involved two nested for loops to compare the values in the sensor array with the reaction matrix. The number of comparisons was the equal to the number of sensors times the number of reactions. Now all we have to do is compare the thisCase byte directly to each possible case in the switch statement. Total number of comparisons is equal to the number of cases. We just cut the number of comparisons for each sensor update down from 24 to 8! This happens ten times per second, so it’s actually kind of significant.

Example Sketch

/*
 * Obstacle avoidance bit math example
 * by Andrew Kramer
 * 8/3/2015
 * 
 * Uses 3 ultrasonic sensors to determine 
 * if nearest obstacle is too close.
 * Outlines 8 possible cases for 
 * positions of obstacles.
 */

#include <NewPing.h>

#define RH_PWM 6 // PWM pin for right hand motor
#define RH_DIR1 9 // direction control for right hand motor
                  // BIN1 pin on motor controller
#define RH_DIR2 8 // direction control for right hand motor
                    // BIN2 pin on motor controller

#define LH_PWM 11 // PWM pin for left hand motor
#define LH_DIR1 13 // direction control for right hand motor
                     // AIN1 pin on motor controller
#define LH_DIR2 12 // direction control for left hand motor
                     // AIN2 pin on motor controller
                     
#define DEFAULT_SPEED 30 // default PWM level for the motors
#define TURN_DISTANCE 20 // distance at which the bot will turn
#define MAX_DISTANCE 200 // max range of sonar sensors
#define NUM_SONAR 3 // number of sonar sensors
#define NUM_CASES 8 // number of reaction cases

NewPing sonar[NUM_SONAR] = { // array of sonar sensor objects
  NewPing(A2, A2, MAX_DISTANCE), // right
  NewPing(A1, A1, MAX_DISTANCE), // front
  NewPing(A0, A0, MAX_DISTANCE) // left
};

/* list of cases
    B0000000  0: no obstacles
    B0000001  1: obstacle to the right
    B0000010  2: obstacle in front
    B0000011  3: obstacles front and right
    B0000100  4: obstacle to the left
    B0000101  5: obstacles left and right
    B0000110  6: obstacles front and left
    B0000111  7: obstacles front, left, and right
*/

byte thisCase = 0;

void setup() {
  for (int pin = 6; pin <= 13; pin++) {
    pinMode(pin, OUTPUT); // set pins 3 through 9 to OUTPUT
  }
  Serial.begin(9600);
}

void loop() {
  updateSensor();
  switch (thisCase) {
    // no obstacles
    case B00000000:
      straightForward();
      break;
    // obstacle to the right
    case B00000001:
      turnLeft(30);
      break;
    // obstacle in front
    case B00000010:
      turnLeft(90);
      break;
    // obstacles front and right
    case B00000011:
      turnLeft(90);
      break;
    // obstacle to the left
    case B00000100:
      turnRight(30);
      break;
    // obstacles left and right
    case B00000101:
      turnLeft(180);
      break;
    // obstacles front and left
    case B00000110:
      turnRight(90);
      break;
    // obstacles front, left, and right
    case B00000111:
      turnLeft(180);
      break;
  }
  delay(100); 
}

void updateSensor() {
    thisCase = 0;
    for (int i = 0; i < NUM_SONAR; i++) {
        int distance = sonar[i].ping_cm();
        if (distance == 0) 
            distance = MAX_DISTANCE;
        if (distance < TURN_DISTANCE)
            thisCase |= (1 << i);
    }
}

void setLeftForward() {
  digitalWrite(LH_DIR1, HIGH);
  digitalWrite(LH_DIR2, LOW);
}

void setRightForward() {
  digitalWrite(RH_DIR1, HIGH);
  digitalWrite(RH_DIR2, LOW);
}

void setBothForward() {
  setLeftForward();
  setRightForward();
}

void setLeftBackward() {
  digitalWrite(LH_DIR1, LOW);
  digitalWrite(LH_DIR2, HIGH);
}

void setRightBackward() {
  digitalWrite(RH_DIR1, LOW);
  digitalWrite(RH_DIR2, HIGH);
}

void setBothBackward() {
  setRightBackward();
  setLeftBackward();
}

void setLeftSpeed(int speed) {
  analogWrite(LH_PWM, speed);
}

void setRightSpeed(int speed) {
  analogWrite(RH_PWM, speed);
}

void setBothSpeeds(int speed) {
  setLeftSpeed(speed);
  setRightSpeed(speed);
}

// sets direction of both motors to forward and sets both speeds
// to default speed
void straightForward() {
  setBothForward();
  setBothSpeeds(DEFAULT_SPEED);
}

// makes a turn by stopping one motor
// accepts an int, 0 or 1 for left or right turn respectively
void turnLeft(int deg) {
  setBothSpeeds(0);
  delay(100);
  setLeftBackward(); // set left motor to run backward
  setBothSpeeds(DEFAULT_SPEED); // set speeds to 1/2 default
  delay(10 * deg); // allow time for the bot to turn
                   // turn time is approx 5ms per degree
  straightForward(); // resume driving forward at default speed
}

void turnRight(int deg) {
  setBothSpeeds(0);
  delay(100);
  setRightBackward(); // set right motor to run backward
  setBothSpeeds(DEFAULT_SPEED); // set speeds to 1/2 default
  delay(10 * deg); // allow time for the bot to turn
                      // turn time is approx 5ms per degree
  straightForward(); // resume driving forward at default speed
}

Sketch Notes

First, the reaction matrix is stored in an array of bytes called cases. The bits in each byte represent a sensor. The right sensor is represented by the least significant bit, the front is the second, and the left is the third least significant bit. There is room in each byte for five more sensors, but we’re only using three.

I think defining the reaction matrix is pretty self explanatory. The only slightly tough thing is building the thisCase byte based on the sensor inputs. We start with a byte consisting of all zeroes: 0b00000000. Then, if the reading from sensor i is less than the reaction distance, we want to change bit i to 1 (we want to flip bit i). We start with 0b00000001 and shift it to the left by i places using the bitwise shift operator: <<. This creates a byte containing a 1 followed by i zeroes. For instance:

int a = 1 << 2; // a is equal to 0b00000100 (or 4)

Then we use the bitwise OR operator to flip the appropriate bit in the thisCase byte. thisCase |= (1 << i); flips bit i in the thisCase byte.

Once the thisCase byte is built it only needs to be compared to every element in the reaction matrix.

Coming Soon

A post on my PID motor control library!

It makes using DC motors much easier. It requires motor encoders to provide feedback to the speed and position control routines, however. So I’ll probably do posts on using encoders and PID control as well.

Obstacle Avoidance

Posted on August 9, 2015 by 1988kramer

At long last I’ve gotten around to doing a post on obstacle avoidance! Thanks to everyone for your patience.

When I started writing this post it had been months since I had last thought about obstacle avoidance. I opened my obstacle avoidance sketch for Colin intending to use it for this post unchanged but it looked terrible. It was a kludgy mess of nested if/else statements. So I took a few hours to totally rewrite my code and make it more efficient and readable. Revisiting old programs can be a great opportunity to reevaluate previous work.

Anyway, the basic idea behind obstacle avoidance is pretty simple. Colin, my mobile robot, can sense objects around himself. If he gets too close to an object he turns away and goes off in another direction. Easy, right?

Wiring Diagram
Program Design
Example Sketch
Video

Preliminaries

We’ll be using HC-SR04 ultrasonic sensors for this tutorial. If you’ve never used ultrasonic sensors before you should take a look at my tutorial. This tutorial also uses DC motors. If you’ve never used DC motors with an Arduino before, take a look at my motor control tutorial.

There is one problem to address right away: the configuration of our sensors. HC-SR04 sensors have a roughly 30° field of view, so with just one sensor Colin won’t be able to see anything to his sides.

Others have solved this problem by mounting a sensor on a servo so the sensor rotates and sweeps out a larger field of view. This instructable is a good example of the technique.

For my purposes it was easier to use an array of three sensors, however. With sensors facing forward, left and right Colin gets a pretty good picture of what’s around him. He still has blind spots at +45° and -45° though, so I’m planning on adding two more sensors.

top

Wiring Diagram

Wiring up the sensors and motors is pretty simple. We really just have to combine the wiring diagrams from the motor control tutorial and the ultrasonic sensor tutorial. Wire the components per the diagram below and you’ll be in good shape.

It’s come to my attention that, on some displays, the color of the TRIG and ECHO wires for the left sonar sensors is very similar to the color of the +5V wire. These wires SHOULD NOT connect, however.

top

Program Design

Before we get into actually writing the obstacle avoidance program we have to decide: how should Colin react to sensor inputs?

The simplest thing we could do is have Colin turn 90º to the left whenever one of his sensors sees an obstacle in front of him. But what if there is also an obstacle to his left? He would turn directly into the obstacle on his left while trying to avoid the one in front of him. What if there is an obstacle on Colin’s left or right but no obstacle in front? Clearly there are several possibilities here.

We need to identify the set of situations Colin might encounter that require him to react. Then we need to identify what those situations look like to Colin in terms of sensor inputs. Lastly, we need to specify an action for Colin to take in each situation.

Let’s assume Colin only needs to take action when an obstacle is within a certain distance. We can call this distance, d_rfor reaction distance. When the distance from one or more of Colin’s sensors to the nearest obstacle is less than d_rColin needs to take action to avoid the obstacle(s). The table below breaks down the situations and sensor inputs that require Colin to react and the action Colin could take.

Reaction Matrix
Obstacle Locations	Left Distance	Front Distance	Right Distance	Response
No Obstacles	> d_r	> d_r	> d_r	Drive forward
Front	> d_r	< d_r	> d_r	Turn left 90°
Front and right	> d_r	< d_r	< d_r	Turn left 90°
Front and left	< d_r	< d_r	> d_r	Turn right 90°
Front, left and right	< d_r	< d_r	< d_r	Turn left 180°
Left and right	< d_r	> d_r	< d_r	Turn left 180°
Right	> d_r	> d_r	< d_r	Turn left 45°
Left	< d_r	> d_r	> d_r	Turn right 45°

Notice that our reaction matrix requires Colin to turn by a number of degrees for most of his reactions. But Colin has no way of knowing how far his wheels have turned, which would be required for him to know how many degrees he’s turned. The best we can do for now is set Colin’s motors to run at different speeds for a certain time interval. Through trial and error we can find the speed differential and time interval required to get a specific amount of turning. This is an approximate solution but we can’t do any better until we add encoders to Colin’s motors.

top

Example Sketch

/*
 * Obstacle avoidance example
 * by Andrew Kramer
 * 8/3/2015
 *
 * Uses 3 ultrasonic sensors to determine
 * if nearest obstacle is too close.
 * Outlines 8 possible cases for
 * positions of obstacles.
 */

#include <NewPing.h>

#define RH_PWM 3 // PWM pin for right hand motor
#define RH_DIR1 4 // direction control for right hand motor
                  // BIN1 pin on motor controller
#define RH_DIR2 5 // direction control for right hand motor
                    // BIN2 pin on motor controller
#define LH_PWM 9 // PWM pin for left hand motor
#define LH_DIR1 7 // direction control for right hand motor
                     // AIN1 pin on motor controller
#define LH_DIR2 8 // direction control for left hand motor
                     // AIN2 pin on motor controller
#define DEFAULT_SPEED 25 // default PWM level for the motors
#define TURN_DIST 25 // distance at which the bot will turn
#define MAX_DISTANCE 200 // max range of sonar sensors
#define NUM_SONAR 3 // number of sonar sensors
#define NUM_CASES 8 // number of reaction cases

#define MS_PER_DEGREE 10 // milliseconds per degree of turning

NewPing sonar[NUM_SONAR] = { // array of sonar sensor objects
  NewPing(13, 13, MAX_DISTANCE), // left
  NewPing(12, 12, MAX_DISTANCE), // front
  NewPing(11, 11, MAX_DISTANCE) // right
};

/*  
 *  stores a bool for each sensor (left, front, and right respectively
 *  true if nearest obstacle is within TURN_DIST
 *  true if not
 */
bool sensor[3] = {false, false, false}; 

// stores all possible sensor states
bool reactions[NUM_CASES][NUM_SONAR] = { 
   {false, false, false}, // 0: no obstacles
   {false, true, false},  // 1: obstacle in front
   {false, true, true},   // 2: obstacles front and right
   {true, true, false},   // 3: obstacles front and left
   {true, true, true},    // 4: obstacles front, left, and right
   {true, false, true},   // 5: obstacles left and right
   {false, false, true},  // 6: obstacle to the right
   {true, false, false} }; // 7: obstacle to the left

void setup() {
  for (int pin = 3; pin <= 9; pin++) {
    pinMode(pin, OUTPUT); // set pins 3 through 9 to OUTPUT
  }
  Serial.begin(9600);
}

void loop() {
  updateSensor();
  switch (compareCases()) {    
    case 0: // no obstacles
      straightForward();
      break;
    case 1: // obstacle in front
      turnLeft(90);
      break;
    case 2: // obstacles front and right
      turnLeft(90);
      break;
    case 3: // obstacles front and left
      turnRight(90);
      break;
    case 4: // obstacles front, left, and right
      turnLeft(180);
      break;
    case 5: // obstacles left and right
      turnLeft(180);
      break;
    case 6: // obstacle to the right
      turnLeft(30);
      break;
    case 7: // obstacle to the left
      turnRight(30);
      break;
  }
  delay(100);
}

void updateSensor() {
  for (int i = 0; i < NUM_SONAR; i++) {
    int dist = sonar[i].ping_cm();
    // if sonar returns 0 nearest obstacle is out of range
    if (dist == 0) sensor[i] = false;
    else sensor[i] = dist < TURN_DIST;
  }
}

int compareCases() {
  for (int i = 0; i < NUM_CASES; i++) {
    bool flag = true;
    for (int j = 0; j < NUM_SONAR; j++) {
      if (reactions[i][j] != sensor[j]) flag = false;
    }
    if (flag) return i;
  }
}

void setLeftForward() {
  digitalWrite(LH_DIR1, HIGH);
  digitalWrite(LH_DIR2, LOW);
}

void setRightForward() {
  digitalWrite(RH_DIR1, HIGH);
  digitalWrite(RH_DIR2, LOW);
}

void setBothForward() {
  setLeftForward();
  setRightForward();
}

void setLeftBackward() {
  digitalWrite(LH_DIR1, LOW);
  digitalWrite(LH_DIR2, HIGH);
}



void setRightBackward() {
  digitalWrite(RH_DIR1, LOW);
  digitalWrite(RH_DIR2, HIGH);
}

void setBothBackward() {
  setRightBackward();
  setLeftBackward();
}

void setLeftSpeed(int speed) {
  analogWrite(LH_PWM, speed);
}

void setRightSpeed(int speed) {
  analogWrite(RH_PWM, speed);
}

void setBothSpeeds(int speed) {
  setLeftSpeed(speed);
  setRightSpeed(speed);
}

// sets direction of both motors to forward and sets both speeds
// to default speed
void straightForward() {
  setBothForward();
  setBothSpeeds(DEFAULT_SPEED);
}

void turnLeft(int deg) {
  setBothSpeeds(0);
  delay(100); // delay to allow motors to stop before direction change
  setLeftBackward();
  setBothSpeeds(DEFAULT_SPEED);
  delay(MS_PER_DEGREE * deg); // allow time for the bot to turn
  straightForward(); // resume driving forward at default speed
}

void turnRight(int deg) {
  setBothSpeeds(0);
  delay(100); // delay to allow motors to stop before direction change 
  setRightBackward(); 
  setBothSpeeds(DEFAULT_SPEED); 
  delay(MS_PER_DEGREE * deg); // allow time for the bot to turn
  straightForward(); // resume driving forward at default speed
}

You probably won’t be able to load up the code on your differential drive robot and run it, even if you have it wired properly. Depending on how you have the motors wired, one or both of them might run backward. To fix this you should just swap the values in the DIR1 and DIR2 fields for the problematic motor. Also, you may have to adjust the value of MS_PER_DEGREE to get accurate turning.

You’ll notice most of the code in the above sketch is devoted to simply controlling the two motors: setting motor directions, PWM levels, etc. In fact, very little of the above sketch codes for higher level behaviors like deciding when and how to react to obstacles. This makes the sketch more difficult to read and it will only get worse when we add encoders to the mix.

To fix this I’ve developed a motor control library. Encapsulating the motor control code in separate motor objects allows us to focus on programming high-level behaviors without worrying too much about how we’re controlling the motors. I’ll present my motor control library (and make it available for download) in my next post.

top

Video

Below you can see a video of Colin running the above example sketch.

You’ll notice that Colin always stops before making a turn. This slows him down and makes his behavior appear jerky. New methods could be added to the sketch to allow Colin to turn while still moving forward in some situations. These methods would simply slow down the wheel on the inside of the turn rather than reversing it. This could be useful when Colin approaches an obstacle obliquely. In this case only a minor course correction is required so a small, smooth turn is better than stopping, rotating, and starting forward again.

That’s all for today. Check back in a couple of weeks for posts on my motor control library and a refinement to obstacle avoidance that uses bit math!

top

Ultrasonic Sensors

Posted on June 16, 2015 by 1988kramer

If you’ve been through my simple motor control tutorial you now know how to control the input voltage to DC motors using an Arduino. In a differential drive robot like Colin, this means driving in a (nearly) straight line, a circle, or some other preset path. It would be more interesting if we could make a robot that reacts to its surroundings. For this we need sensors and ultrasonic sonar sensors are a good place to start.

Ultrasonic sensors determine the distance between themselves and the nearest obstacle by emitting an ultrasound pulse and timing how long it takes for that pulse to be reflected off the nearest obstacle and back to the sensor. They are easy to use, accurate, and they can be extremely cheap.

They do have limitations, however. Sound reflects best off of hard, smooth objects, so soft or uneven surfaces are not detected very well. Most kinds of fabric are basically invisible to an ultrasonic sensor. Reflection is another problem. To work properly, the sensor needs an obstacle to reflect its emitted pulse straight back to the sensor. If the sensor isn’t aimed straight at a perpendicular surface, the reflected pulse could miss the sensor entirely. In this case the sensor would not detect the obstacle. The datasheets for most sensors specify an angle within which the sensor will be able to reliably detect obstacles.

HC-SR04 sensor

For this tutorial I’ll be using an HC-SR04 sensor. The datasheet for the sensor can be found here. Note that it will only reliably detect obstacles within +/- 15 degrees from perpendicular to the face of the sensor. The HC-SR04 definitely isn’t the best ultrasonic sensor out there. More accurate, longer range sensors exist. You cannot, however, find a cheaper ultrasonic sensor than the HC-SR04. If you shop around on Amazon you can find them for $1-3 apiece.

Before I get into the actual tutorial I should mention I’m making use of Tim Eckel’s NewPing library for Arduino here. It makes using these sensors a complete snap. You should definitely take a look at the tutorial on the Arduino Playground in addition to this one. If you don’t have experience installing or using new libraries in Arduino, take a look at this tutorial.

Single Sensor Example

We’ll start out by wiring up and testing a single HC-SR04 sensor. You’ll want to wire it up as in the diagram below.

Wiring diagram for a single HC-SR04 sensor.

Note the Trig and Echo pins on the sensor can be connected to different pins on the Arduino. The NewPing library makes it possible to connect the Trig and Echo pins to the same Arduino pin. This is great because it means the HC-SR04 doesn’t occupy as many I/O pins.

Note also there is no power source pictured on the above diagram. For the purposes of this exercise we can power the Arduino through its USB port, so no external power source is required.

Example Sketch

#include <NewPing.h>

#define TRIGGER_PIN 2
#define ECHO_PIN 2
#define MAX_DISTANCE 200 // max distance the sensor will return

NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE); // declare a NewPing object

void setup() {
  Serial.begin(115200);
}

void loop() {
  delay(50);
  int uS = sonar.ping(); 
  Serial.print("Ping: ");
  Serial.print(uS / US_ROUNDTRIP_CM); // convert ping time to distance in cm
  Serial.println("cm");
}

Load the above sketch on your Arduino and open a terminal window. Make sure the baud rate of the window is the same as the baud rate in your Serial.begin(); statement.

When initializing a NewPing object use the following form: NewPing(triggerPin, echoPin, distanceLimit); If you’re using the same pin for the trigger and echo just set triggerPin and echoPin to the same pin.

The example sketch will display the distance from the sensor to the nearest obstacle in centimeters every 50 milliseconds. If the nearest obstacle is beyond the distanceLimit or if the nearest obstacle is not detectable then sonar.Ping() will return 0.

Play around with the single sensor for a bit. Have some fun with it. Eventually start to realize how limited a single sensor is. A robot that can only see directly in front of itself can’t see obstacles at its sides. So if it approaches an obstacle at an angle, the obstacle won’t be detected. So what should we do? Add more sensors, of course!

Three Sensor Example

Now that we’ve got a single sensor up and running it should be a simple matter to add two more sensors. It can be useful to have an array of sensors when we’re trying to detect obstacles around a mobile robot like Colin. Wire the sensors up as in the diagram below.

Wiring diagram for three HC-SR04 sensors.

Example Code

#include <NewPing.h>

// trigger and echo pins for each sensor
#define SONAR1 2
#define SONAR2 3
#define SONAR3 4
#define MAX_DISTANCE 200 // maximum distance for sensors
#define NUM_SONAR 3 // number of sonar sensors

NewPing sonar[NUM_SONAR] = { // array of sonar sensor objects
  NewPing(SONAR1, SONAR1, MAX_DISTANCE),
  NewPing(SONAR2, SONAR2, MAX_DISTANCE),
  NewPing(SONAR3, SONAR3, MAX_DISTANCE)
};

int distance[NUM_SONAR]; // array stores distances for each
                         // sensor in cm

void setup() {
  Serial.begin(115200);
}

void loop() {
  delay(50);
  updateSonar(); // update the distance array
  // print all distances
  Serial.print("Sonar 1: ");
  Serial.print(distance[0]);
  Serial.print("  Sonar 2: ");
  Serial.print(distance[1]);
  Serial.print("  Sonar 3: ");
  Serial.println(distance[2]);
}

// takes a new reading from each sensor and updates the
// distance array
void updateSonar() {
  for (int i = 0; i < NUM_SONAR; i++) {
    distance[i] = sonar[i].ping_cm(); // update distance
    // sonar sensors return 0 if no obstacle is detected
    // change distance to max value if no obstacle is detected
    if (distance[i] == 0)
      distance[i] = MAX_DISTANCE;
  }
}

Note the step in the updateSonar() method that checks if the distance returned by sonar[i].ping_cm() is 0, meaning the nearest obstacle is probably out of range. Technically it’s possible there is an obstacle 0cm away from the sensor, but that is not very likely. If the value returned by sonar[i].ping_cm() is 0, we switch it to MAX_DISTANCE .

Now we can control 3 sonar sensors simultaneously. We can use this in conjunction with our ability to control motors to make a robot that has autonomous behavior! One of the simpler things we can do with these two tools is program an obstacle avoidance routine. If that interests you stay tuned for my next tutorial, because I’m planning to do in on that very topic!

ICRA 2015 days 2-5

Posted on June 1, 2015 by 1988kramer

I’ve been at the conference on robotics and automation for five days now and all I can say is: “Did you know robots can play football?!”

https://www.youtube.com/watch?v=XLKKbz2mNyo

I stopped by the Robotis booth at the exhibition during lunch one day and they showed me the above video. Robotis makes the Darwin OP robots that make up the American team in the above video. Not only are they adorable, they’re also frighteningly smart. Their goal-blocking behaviors are pretty funny too.

On Wednesday I got to hear a talk from Peter Hart on making Shakey, one of the first autonomous mobile robots. Colin shares Shakey’s basic physical design and I think I can learn a lot from how Shakey’s software was organized. Shakey used a hierarchical system where one program controls low level functions like moving and sensing and separate programs handle motion planning, map building and localization. It seems like a good way to organize Colin’s functions. That’s a long way off though. I’m still busy writing Colin’s motor control library.

On Friday there were two sessions on Simultaneous Localization and Mapping (SLaM). I made sure to attend both of them since my ultimate goal for Colin is to program him for SLaM.

It seems a lot of implementations of SLaM rely on vision via Microsoft Kinect sensors. Those are out of my league for the moment; I have no idea how to program for them and Colin does not have the power to run any such program anyway. Laser rangefinders and LiDAR are also popular and, while I could potentially add one to Colin, they are expensive as hell. I’m still getting some good ideas though. Hopefully I can implement them on a robot that uses cheap ultrasonic and touch sensors instead of fancy RGB+D cameras.

Saturday was my favorite day at the conference by far. I attended an all-day workshop on building a career in robotics research called “Becoming a Robot Guru.” The speakers and panelists got me very excited about working toward a career in robotics. I was encouraged to learn that one doesn’t have to follow the traditional academic career path to be a robotics researcher. I have been worried lately that it may be too late for me to start a PhD in computer science or robotics, but many of the speakers on Saturday took more than a decade to start their PhD after they completed their bachelor’s degree. That gives me hope that a career in research is still an option for me. I’m starting my master’s degree this fall. All I really have to do is keep working at it and I’ll get there eventually.

Simple Motor Control

Posted on May 28, 2015 by 1988kramer

I’ve finally gotten around to outlining the basics of DC motor control with the Arduino. We won’t be completely starting from scratch. I will assume you have some familiarity with the Arduino and its programming environment. If you’ve never done anything with the Arduino before I’d suggest doing some simple tutorials, like how to blink an LED, first. If you can make the blink tutorial work and wire everything up properly, you should be able to get through the examples here.

I’ll start out with a parts list, then I’ll outline how to control a single motor, and then I’ll move on to dual motor control.

Parts List

1 Arduino; I’ll be using the Duemilanove but the examples should work with the newer Uno and most of the other Arduino variants as well.
1 dual motor driver; I’ll be using the TB6612FNG from Pololu
2 DC motors; I’m using micro metal gearmotors from Pololu
Batteries; I’m using a 4 AA pack that gives around 6V but I’ll discuss other options as well
1 voltage regulator; this can be really handy but it’s not totally necessary
Hookup wire; I suggest you use ribbon cable but, at a minimum you should use several different colors of wire. It’s hard to appreciate how hard it is to wire up electronics using only one color of wire unless you’ve actually tried it.
Finally, some soldering skill will be necessary. There’s a good tutorial here if you’re not familiar. Do a lot of practicing before you try to solder anything you care about.

The Motor Driver

The Arduino’s digital pins cannot provide enough current to run even a small motor. So we need a power source for the motor that can supply a lot of current, but we also need a way to control that power source using the Arduino. This can be done using a simple switching transistor as in this great Adafruit tutorial but using a motor driver circuit like the TB6612FNG has some advantages:

We don’t have to build our own motor control circuit. As long as we wire it up correctly the TB6612FNG works right out of the box.
It allows us to control two motors with the same circuit, so it saves us a bit of complexity for dual motor control.
Most importantly, it gives us software control of the motor’s direction using the AIN1 and AIN2 pins for one motor, and the BIN1, and BIN2 pins for the other:
- If IN1 is driven HIGH and IN2 is LOW the motor will turn forward
- If IN1 is driven LOW and IN2 is HIGH the motor will turn backward
- If IN1 and IN2 are LOW the motor will freewheel
- If IN1 and IN2 are driven HIGH the motor will lock up or be in a “braking” state

The Battery / Voltage Regulator

All of the components in this setup have different power requirements. The motors require around 6V and they draw a lot of current. The Arduino needs 7-12V. It is possible to power the Arduino by connecting a 5V source to the 5V pin, but this bypasses the Arduino’s onboard voltage regulator so you need to make sure you have a regulated 5V source if you want to go that route. Lastly, the motor driver needs 5V for its logic. The motor driver can get its 5V from the 5V pin on the Arduino, but how do we satisfy the different requirements of the motors and the Arduino?

Option 1: Different power sources for the motors and Arduino

A 4 AA battery pack provides between 5.5 and 6.5V and can give a lot of current, which makes it good for the motors. But the Arduino needs 7-12V. Some people solve this by using a separate power source for the Arduino, usually a 9V battery. This is problematic for a couple of reasons. First, the Arduino’s onboard regulator isn’t very efficient when stepping 9V down to 5V and a lot of energy from the 9V battery is lost as waste heat. Second, having 2 separate battery packs adds weight and complexity. Wouldn’t it be much easier if we could power everything using just one battery pack?

Option 2: Voltage regulator

The addition of a voltage regulator like this one can make things much simpler. As in the wiring diagram above, you can power the motors directly from the battery pack and the Arduino can get power from the voltage regulator, which is tuned to output 5V. This approach is simpler, lighter weight, and more efficient than option 1.

A word of warning: do not try to power the motor with the output of the voltage regulator. They will draw more current than the regulator can supply and cause the voltage from the regulator to drop. When this happens your Arduino will reset, which can cause some pretty baffling problems if you don’t know what’s happening. Just power your motor directly from the battery as in the diagram.

Single Motor Control

The most important part of this project is wiring the physical components correctly. As I said in a previous post, I highly recommend wiring everything up on a breadboard. Do not make any soldered connections if you don’t absolutely have to. Rewiring is far easier when you’re using a breadboard. Below is a diagram of a wiring configuration that will work with my example sketch.

breadboard layout for single motor control

Single Motor Example Sketch

It’s finally time for the example code! You should be able to copy and paste the code below into your Arduino IDE if you’ve wired your motor up exactly as I have in the diagram above.

// Single Motor Control example sketch
// by Andrew Kramer
// 5/28/2015

// makes the motor run at 1/2 power

#define PWMA 3 // PWM pin for right hand motor
#define AIN1 4 // direction control pin 1
               // AIN1 on the motor controller
#define AIN2 5 // direction control pin 2 
               // AIN2 pin on motor controller

void setup() {
  // set all pins to output
  for (int pin = 3; pin <= 5; pin++) {
    pinMode(pin, OUTPUT); // set pins 3 through 5 to OUTPUT
  }
  // set motor direction to forward 
  digitalWrite(AIN1, HIGH);
  digitalWrite(AIN2, LOW);
  // set the motor to run at 1/2 power
  analogWrite(PWMA, 128);
}

void loop() {
  // we're just making the motor run at a constant speed,
  // so there's nothing to do here
}

So now we can wire up a motor and feed it a constant amount of voltage. That’s useful, but not very interesting by itself. To make a differential drive robot like Colin, we need to be able to control two motors simultaneously.

Dual Motor Control

The basics of dual motor control are the same as for single motor control, we’re just adding an additional motor. Wire up your second motor as shown in the diagram below.

wiring diagram for dual motor control

Dual Motor Example Sketch

The sketch below, when used in a differential-drive robot will make the robot drive in a small circle. It’s a common problem to load up this example and find one or both of your motors is running backward. If this happens, you can either switch the states of the IN1 and IN2 pins for the backward motor in void setup() . Or, if you want a hardware solution, you can switch the positions of the wires connecting to IN1 and IN2 or OUT1 and OUT2.

// Dual Motor Control example sketch
// by Andrew Kramer
// 5/28/2015

// makes motor A run at 1/4 power and
// motor B run at 1/8 power
// in a differential drive robot this would cause the robot to
// drive in a small circle

#define PWMA 3 // PWM pin for right hand motor
#define AIN1 4 // direction control pin 1 for right motor
#define AIN2 5 // direction control pin 2 for right motor
#define PWMB 9 // PWM pin for left hand motor
#define BIN1 7 // direction control pin 1 for left motor
#define BIN2 8 // direction control pin 2 for left motor

void setup() {
  // set all pins to output
  for (int pin = 3; pin <= 5; pin++) {
    pinMode(pin, OUTPUT); // set pins 3 through 5 to OUTPUT
  }
  for (int pin = 7; pin <= 9; pin++) {
    pinMode(pin, OUTPUT); // set pins 7 through 9 to OUTPUT
  }
  // set both motors to forward 
  digitalWrite(AIN1, HIGH);
  digitalWrite(AIN2, LOW);
  digitalWrite(BIN1, HIGH);
  digitalWrite(BIN2, LOW);
  // set right motor to run at 1/4 power
  analogWrite(PWMA, 64);
  // set left motor to run at 1/8 power 
  analogWrite(PWMB, 32);
}

void loop() {
  // we're just making the motors run at constant speed,
  // so there's nothing to do here
}

Further Work

So now we can give two motors a constant amount of input power. That’s a bit boring, isn’t it? You can take things a bit further by making the motor speed respond to sensor inputs. For example, you could control the power to the motors using a potentiometer. You could make your differential drive robot follow more complicated paths or avoid obstacles using ultrasonic sensors, which I’ll discuss in a later post.

There’s a big problem here though. Just using analogWrite() doesn’t control the motor’s speed, only the voltage to the motor. Even if you give both motors the same input voltage they won’t run at exactly the same speed. So you won’t be able to get a differential drive robot to drive in a straight line. For that we need actual speed control, and for that the motor needs to be able to tell the Arduino how fast it’s turning, and for that we need encoders. I’ll write a post about speed control using encoders like these soon.

Bot Blog

building my robot army one sketch at a time

Category Archives: Robotics

Colin’s Spooky Brain Transplant!

Moving the Arduino to the Breadboard

Adding Computing Power!

Odometry With Arduino

Theory Basics

Odometry Code

Further Work

PID Motor Control

Program Structure

Control Loop Basics

Proportional Control

Integral and Derivative Terms

PID Adjustment Equation

The Code

Derivative Kick

Example Sketch

Implementation Notes

I Finally Learned Git!

Motor Encoders with Arduino

Bitwise Obstacle Avoidance Tweak

Obstacle Avoidance

Preliminaries

Wiring Diagram

Program Design

Reaction Matrix

Example Sketch

Video

Ultrasonic Sensors

ICRA 2015 days 2-5

Simple Motor Control