Using Simple Wheel Encoders with an UNO R3 to Drive a Robot Straight

I am relatively new to Arduino and trying to use some simple wheel encoders to develop a sketch that will correct wheel power values based on the encoder values. I have created a sketch shown here to try to accomplish this task. The results are quite inconsistent and I am thinking I may have some logical errors in my code.

Any suggestions are greatly appreciated.

/* Created by Steve Cline on 17 December 2017
 *  This sketch is designed to be used with the DFRobot Baron 4WD robot.
 *  Attached additional equipment includes:
 *    Arduino UNO R3
 *    AdaFruit MotorShield V2.3
 *    Battery pack with 8 AA NiMH cells attached to the motor shield
 *    Battary pack with 5 AA NiMH cells attached to the microcontroller
 *    4 DC 130 motors
 *    2 non-contact wheel encoders with 20 PPR resolution attached to back wheels
 *  Modify the value for the variable "drive" to modify behavior.  
#include <Adafruit_MotorShield.h>

// Create the motor shield object with the default I2C address
Adafruit_MotorShield AFMS = Adafruit_MotorShield();

// Select which 'port' M1, M2, M3 or M4
Adafruit_DCMotor *LF = AFMS.getMotor(1);
Adafruit_DCMotor *LR = AFMS.getMotor(2);
Adafruit_DCMotor *RR = AFMS.getMotor(3);
Adafruit_DCMotor *RF = AFMS.getMotor(4);

// encoder pins
const int LEncoder = 2;
const int REncoder = 3;

// general variables for settings on the robot
int drive = 120;

// variables to store encoder values
int LCount = 0;
int RCount = 0;
int currentLeft = 0;
int currentRight = 0;
int pastLeft = 0;
int pastRight = 0;

int masterPower = 90;
int slavePower = 90;
int error = 0;
int kp = 5;

void setup() {      
  AFMS.begin();     // create with the default frequency 1.6KHz



  delay(5000);                    //Wait 5 seconds to set the robot on the course

  // drive forward

  // drive forward until the encoder returns the preset value on both rear wheels
  while (LCount <= drive && RCount <= drive){


    currentLeft = digitalRead(LEncoder);            // read the left encoder
    currentRight = digitalRead(REncoder);           // read the right encoder
    // test the state of the encoders 
    if (pastLeft == 0 && currentLeft == 1){
      pastLeft = 1;
      LCount += 1;
    else if (pastLeft == 1 && currentLeft == 0){
      pastLeft = 0;
    else if (pastRight == 0 && currentRight == 1){
      pastRight = 1;
      RCount += 1;
    else if (pastRight == 1 && currentRight == 0){
      pastRight = 0;

    error = LCount - RCount;
    slavePower += error / kp;

  // stop and wait


void loop() {


Reading encoders by polling is not a good way to do it, if the code is doing other things. It is not a good approach if the encoders are outputting a lot of pulses.

How many pulses per revolution are your encoders outputting?

I am not entirely sure but I think the specs say 20 PPR. Is that pulses per revolution? Here is the actual encoder at RobotShop: Gravity Propulsion Kit (Wheels / Encoder / Motors) - RobotShop

What would be an alternative approach to polling that you would suggest?

You can use an interrupt to detect when a pulse is produced. From the picture in your link there seem to be 10 slots in the encoders which would imply 10 pulses per revolution. The Uno has 2 external interrupt pins so you can use one for each wheel. look up attachInterrupt() in the Reference section

Counting encoder steps is unlikely to be sufficient to allow a robot to navigate reliably. Small differences in slippage between the two wheels will quickly add up to a lost robot. You really need some external reference point such as a white line on the ground. And with that you probably would not need encoders.