Skip to content

Instantly share code, notes, and snippets.

Last active October 25, 2023 17:13
Show Gist options
  • Star 23 You must be signed in to star a gist
  • Fork 15 You must be signed in to fork a gist
  • Save ShawnHymel/1de08ffaca990b65fade81cb8d01a44a to your computer and use it in GitHub Desktop.
Save ShawnHymel/1de08ffaca990b65fade81cb8d01a44a to your computer and use it in GitHub Desktop.
Use two magnetic encoders to make a robot drive in a straight line for 1 meter
* Adventures in Science: Fred Bot
* SparkFun Electronics
* Author: M. Hord (October 8, 2013)
* Modified by: B. Huang (October 31, 2014)
* Modified by: Shawn Hymel (July 21, 2017)
* Use two magnetic encoders on Fred's motor shafts (48:1
* gearbox, 60mm wheels) to make him move in a straight line for
* 1m.
* License: Beerware (
// Parameters
const int drive_distance = 100; // cm
const int motor_power = 200; // 0-255
const int motor_offset = 5; // Diff. when driving straight
const int wheel_d = 60; // Wheel diameter (mm)
const float wheel_c = PI * wheel_d; // Wheel circumference (mm)
const int counts_per_rev = 384; // (4 pairs N-S) * (48:1 gearbox) * (2 falling/rising edges) = 384
// Pins
const int enc_l_pin = 2; // Motor A
const int enc_r_pin = 3; // Motor B
const int pwma_pin = 5;
const int ain1_pin = 9;
const int ain2_pin = 4;
const int pwmb_pin = 6;
const int bin1_pin = 7;
const int bin2_pin = 8;
const int stby_pin = 10;
// Globals
volatile unsigned long enc_l = 0;
volatile unsigned long enc_r = 0;
void setup() {
// Debug
// Set up pins
pinMode(enc_l_pin, INPUT_PULLUP);
pinMode(enc_r_pin, INPUT_PULLUP);
pinMode(pwma_pin, OUTPUT);
pinMode(ain1_pin, OUTPUT);
pinMode(ain2_pin, OUTPUT);
pinMode(pwmb_pin, OUTPUT);
pinMode(bin1_pin, OUTPUT);
pinMode(bin2_pin, OUTPUT);
pinMode(stby_pin, OUTPUT);
// Set up interrupts
attachInterrupt(digitalPinToInterrupt(enc_l_pin), countLeft, CHANGE);
attachInterrupt(digitalPinToInterrupt(enc_r_pin), countRight, CHANGE);
// Drive straight
driveStraight(drive_distance, motor_power);
void loop() {
// Do nothing
void driveStraight(float dist, int power) {
unsigned long num_ticks_l;
unsigned long num_ticks_r;
// Set initial motor power
int power_l = motor_power;
int power_r = motor_power;
// Used to determine which way to turn to adjust
unsigned long diff_l;
unsigned long diff_r;
// Reset encoder counts
enc_l = 0;
enc_r = 0;
// Remember previous encoder counts
unsigned long enc_l_prev = enc_l;
unsigned long enc_r_prev = enc_r;
// Calculate target number of ticks
float num_rev = (dist * 10) / wheel_c; // Convert to mm
unsigned long target_count = num_rev * counts_per_rev;
// Debug
Serial.print("Driving for ");
Serial.print(" cm (");
Serial.print(" ticks) at ");
Serial.println(" motor power");
// Drive until one of the encoders reaches desired count
while ( (enc_l < target_count) && (enc_r < target_count) ) {
// Sample number of encoder ticks
num_ticks_l = enc_l;
num_ticks_r = enc_r;
// Print out current number of ticks
// Drive
drive(power_l, power_r);
// Number of ticks counted since last time
diff_l = num_ticks_l - enc_l_prev;
diff_r = num_ticks_r - enc_r_prev;
// Store current tick counter for next time
enc_l_prev = num_ticks_l;
enc_r_prev = num_ticks_r;
// If left is faster, slow it down and speed up right
if ( diff_l > diff_r ) {
power_l -= motor_offset;
power_r += motor_offset;
// If right is faster, slow it down and speed up left
if ( diff_l < diff_r ) {
power_l += motor_offset;
power_r -= motor_offset;
// Brief pause to let motors respond
// Brake
void enableMotors(boolean en) {
if ( en ) {
digitalWrite(stby_pin, HIGH);
} else {
digitalWrite(stby_pin, LOW);
void drive(int power_a, int power_b) {
// Constrain power to between -255 and 255
power_a = constrain(power_a, -255, 255);
power_b = constrain(power_b, -255, 255);
// Left motor direction
if ( power_a < 0 ) {
digitalWrite(ain1_pin, LOW);
digitalWrite(ain2_pin, HIGH);
} else {
digitalWrite(ain1_pin, HIGH);
digitalWrite(ain2_pin, LOW);
// Right motor direction
if ( power_b < 0 ) {
digitalWrite(bin1_pin, LOW);
digitalWrite(bin2_pin, HIGH);
} else {
digitalWrite(bin1_pin, HIGH);
digitalWrite(bin2_pin, LOW);
// Set speed
analogWrite(pwma_pin, abs(power_a));
analogWrite(pwmb_pin, abs(power_b));
void brake() {
digitalWrite(ain1_pin, LOW);
digitalWrite(ain2_pin, LOW);
digitalWrite(bin1_pin, LOW);
digitalWrite(bin2_pin, LOW);
analogWrite(pwma_pin, 0);
analogWrite(pwmb_pin, 0);
void countLeft() {
void countRight() {
Copy link

Sir in an Encoder we have two channel where as in the program only one channel from each motor is used..

Could you please explain it.?

Copy link

what does enableMotors function do to the robot?
void enableMotors(boolean en) { if ( en ) { digitalWrite(stby_pin, HIGH); } else { digitalWrite(stby_pin, LOW); } }

Copy link

mekaci89 commented Mar 8, 2019

My each encoder has 2 pins on each motor driver (hall effect sensor encoder ones) and so it is 4 pins at total. The given example is here only for the one encoder pin and the module is used different too. Give example with every possibility at least. Already there is not many example regarding encoders for such functionality.

Copy link

i am having optical encoder and it does not have any such pins as pwm and the other one
ca u share the optical code please

Copy link

what does enableMotors function do to the robot?
void enableMotors(boolean en) { if ( en ) { digitalWrite(stby_pin, HIGH); } else { digitalWrite(stby_pin, LOW); } }

in this case his motor driver is in standby mode if stby pin value isn't set to HIGH. If your driver doesn't have this pin you can do it without this function

Copy link

Here are my two cents. For whatever it is worth.

I got this code to work after a tiny bit of messing around. I designed a circuit in Tinkercad to test it first, before I built a physical model.

For some reason I couldn't get Serial.println to return any values until I turned down the delay on line 59 from 1000 to 75. Then I had a reading and the code worked.

The motor driver I used didn't have a standby. So I didn't need to initialise pin 10 or use the EnableMotors function from line 145 or use the function call on line 60. Easy peasy.

When I finally built the model in real life, I had a problem where the robot would detect slippage or a difference in wheel speed then spin a wheel in the opposite direction. This was making it impossible to use. To get around this I changed the constrain power on lines 156 and 157 so that the values couldn't go negative anymore. Got rid of the -255 and replaced it with a 0. It worked for me.

This code saved my bacon and I hope that my comments here save someone else a few hours of their day.


Copy link

can u share the code to run a single encoder motor to required limits

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment