Robot driving to the left when it should be going straight

Hello. I’m using the Sparkfun Encoder Kit to have an Arduino Robot drive a straight line. However, when I run the code the robot immediately starts turning to the left. I believe this is because the right motor is moving faster than the left motor, but the code I wrote should be compensating for that, and it is not. My code is listed below:

#include <RedBot.h>

RedBotEncoder encoder = RedBotEncoder(A4, A5);
int countsPerRev = 4;   // 4 pairs of N-S x 48:1 gearbox = 192 ticks per wheel rev

float wheelDiam = 2.56;  // diam = 65mm / 25.4 mm/in
float wheelCirc = PI*wheelDiam;  // Redbot wheel circumference = pi*D

// Motor control pins : L298N H bridge
const int enAPin = 6; // Left motor PWM speed control
const int in1Pin = 7; // Left motor Direction 1
const int in2Pin = 5; // Left motor Direction 2
const int in3Pin = 4; // Right motor Direction 1
const int in4Pin = 2; // Right motor Direction 2
const int enBPin = 3; // Right motor PWM speed control
enum Motor { L, R };

void go( enum Motor m, int speed)
{
  digitalWrite (m == L ? in1Pin : in3Pin , speed > 0 ? HIGH : LOW );
  digitalWrite (m == L ? in2Pin : in4Pin , speed <= 0 ? HIGH : LOW );
  analogWrite(m == L ? enAPin : enBPin, speed < 0 ? -speed : speed);
}

void setup()
{
  //pinMode(buttonPin, INPUT_PULLUP);
  Serial.begin(9600);
}

void loop(void)
{
  // set the power for left & right motors on button press
  //if (digitalRead(buttonPin) == LOW)
  //{
    driveStraight(12, 200);    
  //}
}

void driveStraight(float distance, int motorPower)
{
  long lCount = 0;
  long rCount = 0;
  long targetCount;
  float numRev;

  // variables for tracking the left and right encoder counts
  long prevlCount, prevrCount;

  long lDiff, rDiff;  // diff between current encoder count and previous count

  // variables for setting left and right motor power
  int leftPower = motorPower;
  int rightPower = motorPower;

  // variable used to offset motor power on right vs left to keep straight.
  int offset = 5;  // offset amount to compensate Right vs. Left drive

  numRev = distance / wheelCirc;  // calculate the target # of rotations
  targetCount = numRev * countsPerRev;    // calculate the target count

  // debug
  Serial.print("driveStraight() ");
  Serial.print(distance);
  Serial.print(" inches at ");
  Serial.print(motorPower);
  Serial.println(" power.");

  Serial.print("Target: ");
  Serial.print(numRev, 3);
  Serial.println(" revolutions.");
  Serial.println();

  // print out header
  Serial.print("Left\t");   // "Left" and tab
  Serial.print("Right\t");  // "Right" and tab
  Serial.println("Target count");
  Serial.println("============================");

  encoder.clearEnc(BOTH);    // clear the encoder count
  delay(100);  // short delay before starting the motors.

  go(L, motorPower);
  go(R, motorPower);

  while (lCount < targetCount)
  {
    // while the right encoder is less than the target count -- debug print 
    // the encoder values and wait -- this is a holding loop.
    lCount = encoder.getTicks(LEFT);
    rCount = encoder.getTicks(RIGHT);
    Serial.print(lCount);
    Serial.print("\t");
    Serial.print(rCount);
    Serial.print("\t");
    Serial.println(targetCount);

    go(L, leftPower);
    go(R, rightPower);

    // calculate the rotation "speed" as a difference in the count from previous cycle.
    lDiff = (lCount - prevlCount);
    rDiff = (rCount - prevrCount);

    // store the current count as the "previous" count for the next cycle.
    prevlCount = lCount;
    prevrCount = rCount;

    // if left is faster than the right, slow down the left / speed up right
    if (lDiff > rDiff) 
    {
      leftPower = leftPower - offset;
      rightPower = rightPower + offset;
    }
    // if right is faster than the left, speed up the left / slow down right
    else if (lDiff < rDiff) 
    {
      leftPower = leftPower + offset;  
      rightPower = rightPower - offset;
    }
    delay(50);  // short delay to give motors a chance to respond.
  }
  // now apply "brakes" to stop the motors.
  go(L, 0);
  go(R, 0); 
}

I tried adding to the left motor power and subtracting from the right motor power initially, but it didn’t stop the turning. I also tried changing the offset value in the DriveStraight function, but that didn’t stop the turning either. I’m at a loss as to why my code is not making the robot go straight and I would appreciate any help on this issue.

Hi,
Can you please post links to the encoder and the Arduino Robot kit?

Have you written code JUST to read the encoders to prove you are reading them correctly?
How many encoders do you have?
Two I hope, one on each motor shaft, although I can only see reference in your code to one encoder.

Thanks.. Tom.... :slight_smile:

When you write a simple sketch to do nothing but run both motors at the same PWM value, does it go reasonably straight? If not, you may have a problem more fundamental than your steering control. Maybe your left motor is faulty and much slower than your right motor.

Yep. Try swapping motor connections and see if it steers the other way.

TomGeorge:
Hi,
Can you please post links to the encoder and the Arduino Robot kit?

Have you written code JUST to read the encoders to prove you are reading them correctly?
How many encoders do you have?
Two I hope, one on each motor shaft, although I can only see reference in your code to one encoder.

Thanks.. Tom....

Here's the encoder: Wheel Encoder Kit - ROB-12629 - SparkFun Electronics
And here's the Arduino Robot kit: https://www.amazon.com/VKmaker-Avoidance-tracking-Ultrasonic-tutorial/dp/B01CXVA6IO/ref=as_li_ss_tl?ie=UTF8&linkCode=ll1&tag=insider04-20&linkId=24caa5b86aa4c80a84b450088f9f8aaa
I wrote code to just run the motors and read the encoder values. What I got from this was data showing that my right motor was moving faster than the left. I have two encoders, one on each wheel. In my code, it initializes both in one line.

You could use some additional serial output in the while loop to be tracking the counts and power values with passes through.

Are your encoders on the geared output, or on the motor shaft? Do you have 4 or 192 counts per revolution of the wheel?

Hi,
So its not actually and encoder, its a chopper, it cannot show direction, just count distance.

Have you got some code that JUST counts and displays on the IDE monitor the count of each chopper?
Have you done some testing with the robot off the ground, so it is freewheeling?

I usually run Serial.begin(115200) too.

Thanks.. Tom.. :slight_smile:

cattledog:
You could use some additional serial output in the while loop to be tracking the counts and power values with passes through.

Are your encoders on the geared output, or on the motor shaft? Do you have 4 or 192 counts per revolution of the wheel?

The encoders are mounted to the geared output, so it's 4 counts per revolution.

robotmaker67:
I wrote code to just run the motors and read the encoder values. What I got from this was data showing that my right motor was moving faster than the left.

That's not unusual - your motors won't be exactly the same - that's why you need the encoders so you can compensate in your code.

Most people use PID to correct the speed difference. With your encoder, (and starting from your code) the general approach would be outlined as follows

float Kp = 1.0; //starting value for the proportional controller constant
// in loop() ...

    // calculate the rotation "speed" as a difference in the count from previous cycle.
    lDiff = (lCount - prevlCount);
    rDiff = (rCount - prevrCount);

    // store the current count as the "previous" count for the next cycle.
    prevlCount = lCount;
    prevrCount = rCount;

int Diff = lDiff-rDiff;  //if Diff is positive the left side gained more counts and is moving faster

   go(L, base_speed - Kp*Diff);  //slow down left motor if Diff is positive, speed up right
   go(R, base_speed + Kp*Diff); //adjust Kp by experiment, starting with 1.0

...

Google "PID tuning" for tutorials on how to select the K values. You may need a Kd as well.

The encoders are mounted to the geared output, so it’s 4 counts per revolution.

I do not think that you have enough resolution to use your adjustment algorithm based on the difference in the number of counts every 100ms. For driveStraight(12, 200); your total target counts are only around 6.

I would think that you need to redesign the algorithm to take the time difference between single counts on the left and right and to make the adjustment based on that.

Alternatively, get the counts on the ungeared output where you can get more resolution.

while (lCount < targetCount)
  {
    // while the right encoder is less than the target count -- debug print
    // the encoder values and wait -- this is a holding loop.
    lCount = encoder.getTicks(LEFT);
    rCount = encoder.getTicks(RIGHT);
    Serial.print(lCount);
    Serial.print("\t");
    Serial.print(rCount);
    Serial.print("\t");
    Serial.println(targetCount);

    go(L, leftPower);
    go(R, rightPower);

    // calculate the rotation "speed" as a difference in the count from previous cycle.
    lDiff = (lCount - prevlCount);
    rDiff = (rCount - prevrCount);

    // store the current count as the "previous" count for the next cycle.
    prevlCount = lCount;
    prevrCount = rCount;

    // if left is faster than the right, slow down the left / speed up right
    if (lDiff > rDiff)
    {
      leftPower = leftPower - offset;
      rightPower = rightPower + offset;
    }
    // if right is faster than the left, speed up the left / slow down right
    else if (lDiff < rDiff)
    {
      leftPower = leftPower + offset; 
      rightPower = rightPower - offset;
    }
    delay(50);  // short delay to give motors a chance to respond.
  }