Accurate 90 degree turns and straight lines with pololu 3pi

Robotics and electronics discussion Software and microcontrollers
1

I am coding a 3pi robot to follow a precise path consisting of straight lines and 90 degree turns. I am initially finding some success in using a PID algorithm to tune the straight lines, with a target set at having the gyroscope at 0 degrees. However, I am having issues:

  1. I am noticing that the gyroscope drifts in a very noticeable way over time, even when I give it around 15 seconds to calibrate. Is there any fix for this? Could I perhaps combine the gyro with other IMU sensors or even just abandon it to use the magnetometer (I would rather not do this however because I don’t know how the magnetic fields might change in different settings)
  2. I find that my 90 degree turns (which I execute using the encoders) are a tad off. Is there a fix for this? I have been trying to feed the error back into my PID, but because the gyro drifts over time, this also seems to be off a little bit.
    I know the 3pi is capable of really accurate turns and straight lines(as in this example Pololu - 8.f. Improving the Maze-Solving Code) so I am wondering if using some other sensor might be a better idea.

Thank you!

2

It is a fact of life that gyros drift, especially consumer grade gyros, and the drift is temperature dependent. Only sensor fusion can help with that, and for horizontal turns, a horizon reference like a magnetometer is required.

The magnetometer is very useful but must be carefully calibrated as well. The best tutorial is this one: Tutorial: How to calibrate a compass (and accelerometer) with Arduino | Underwater Arduino Data Loggers

Fully worked example Correcting the Balboa magnetometer

You could also fuse information from wheel encoders, keeping in mind that wheel slip is almost unavoidable.

3

Thank you so much for the response! How might I go about “fusing” data from the magnetometer to the gyro data?

4

One approach is the complementary filter, although I don’t know of an example for a magnetometer. There are many examples for pitch and roll, fusing the gyro and accelerometer data to level aircraft, and it would not be hard to modify one of those to give the yaw angle using the magnetometer.

I prefer the Mahony AHRS filter, which is much more general and handles all three orientation angles, with a “quaternion” intermediate. I don’t know if one has been written for the LSM6DS33/LIS3MDL combination, but if so, that is all you need.

5

Once again, thank you so much for your help Jim. After some reading, I’ve concluded that the complementary filter with the gyro and accelerometer sounds like the best approach, given that the magnetometer might be inconsistent. How might I go about implementing a complementary filter for my 3pi bot?

EDIT: I’m realizing that I spoke too soon, and what I should be doing is using the magnetometer instead of the accelerometer as I want yaw, not pitch or roll. Sorry about that!

6

A web search will find tutorials describing what other people have done. The search phrase “complementary filter gyro magnetometer” returns promising results.

Update: I did some searching and could not find an example of a gyro/magnetometer complementary filter that would not require a lot of modification.

However, this link describes an implementation of the Mahony filter for the combination of sensors used in the 3PI+: https://forums.adafruit.com/viewtopic.php?t=197345

7

I will look at the link you provided Jim, thank you! Even though I would like to avoid using the magnetometer, it seems as though that might have to be a last resort.

Despite the drift, I’m still trying to work at making accurate turns with solely the gyro for simplicity, given that with around 15 seconds of calibration I’m able to get it down to roughly 1 degree of drift per 15-20 seconds, which I find roughly acceptable. What I’m finding, though, is that in addition to the drift, the gyro is fairly inaccurate in terms of measuring right angle turns over a short period of time. I find that it’s often off by up to 3-ish degrees, which compounds heavily after a couple of turns and straight line moves. Am I doing something wrong (maybe I made some data type error throwing off precision) or is this also something to be expected when using a gyro? Is there any fix?

8

Sounds like a problem with the code. For help with that, post it using code tags.

9

Here is my code specifically for my left turn. Even after calibrating the gyro for ~10 ish seconds (using the gyro calibration method that Pololu gives in their examples), the final value of the turn angle (which should be pretty close to 90 just eyeballing it) is read out by the gyro in values varying from -88 to 93 degrees (around ±2 or 3 degrees of error sometimes). Given that I’m using the encoders of the robot, I’m expecting that the robot is turning a consistent distance every time (even if there’s inertia, inertia shouldn’t vary between runs). Is there something wrong I’m doing or am I correct in my conclusion that the gyro isn’t super accurate?

void turn_left_90()
{
  display.clear();
  encoders.getCountsAndResetLeft();
  encoders.getCountsAndResetRight();
  //turnSensorReset();
  motors.setSpeeds(0,0);

  display.print(-(int32_t)turnAngle/(float)turnAngle1);

  delay(500);
  
  while (encoders.getCountsRight() < 235 && encoders.getCountsLeft() > -235){
    motors.setSpeeds(-turn90Speed, turn90Speed);
    turnSensorUpdate();
  }

  motors.setSpeeds(0,0);
  turnSensorUpdate();
  
  display.clear();

  //turnAngle = turnAngle90 -(int32_t)turnAngle;
  //turnSensorReset();
  
  while (true){
  display.clear();
  //display.gotoXY(0,1);
  display.print(-(int32_t)turnAngle/float(turnAngle1));
// values deviate pretty badly here
  turnSensorUpdate();
  delay(5);
  }


}
10

The problem is likely in the code you forgot to post. Please post all the code.

11

Sure. Here is my whole file (disregard any errors in the right turn, backwards movement, or the new left turn I’m currently making based on a PID):

#include <Pololu3piPlus32U4.h>
#include <Pololu3piPlus32U4IMU.h>
#include <Wire.h>
#include <PID_v1.h>
#define PI 3.141592
#define ENCODER_COUNTS_PER_REV 358.3  // encoder value for 1rev

using namespace Pololu3piPlus32U4;

OLED display;
ButtonB buttonB;
BumpSensors bumpSensors;
Motors motors;
Encoders encoders;
IMU imu;
#include "TurnSensor.h"

int16_t maxSpeed = 80;
int32_t turnSpeed = 50;
int32_t turn90Speed = 50;

// constants for PID
//double Kp = 1, Ki = -0.0002, Kd = 0.005;

double Kp = 1, Ki = -0.0002, Kd = 0;

boolean hasRun = false;

void moveForwardDistance(float distance_mm){
    // TODO: perhaps add trap motion profile
    int32_t acceleration = 10;

    float mm_per_rev = PI * 32;
    int32_t targetCounts = (distance_mm / mm_per_rev) * ENCODER_COUNTS_PER_REV;
    
    int32_t turnAngleIntegral = 0;
    
    //offset
    //distance_mm -= 60;

    encoders.getCountsAndResetLeft();
    encoders.getCountsAndResetRight();
        
    while (true){
        // add acceleration and deceleration

        int32_t leftCount = encoders.getCountsLeft();
        int32_t rightCount = encoders.getCountsRight();
        int32_t averageCount = (leftCount + rightCount) / 2;

        if (averageCount >= targetCounts){
            break;
        }

        turnSensorUpdate();

        // add to integral term
        // a RIGHT turn of 1 degree reads as -1 degrees **

        turnAngleIntegral += -((int32_t)turnAngle/(float)turnAngle1);

        int32_t turnSpeed = (int32_t)(Kp * (-(int32_t)turnAngle / ((float)turnAngle1)) + Kd*(-0.07 * turnRate) - Ki * (turnAngleIntegral));
        //int32_t turnSpeed = 0;
        Serial.print(-(int32_t)turnAngle / (float)(turnAngle1));
        Serial.print(" ");
        Serial.print(-0.07 * turnRate);
        Serial.print(" ");
        Serial.print(-maxSpeed);
        Serial.print(" ");
        Serial.println(turnAngleIntegral);


        // see that these all must have the same signs
        turnSpeed = constrain(turnSpeed, -maxSpeed, maxSpeed);


        motors.setSpeeds((int16_t)maxSpeed-turnSpeed, (int16_t)maxSpeed+turnSpeed);

        display.clear();
        // display.println(turnSpeed);
        display.println(-(int32_t)turnAngle/turnAngle1);
        display.println((int16_t)turnSpeed);

    }
        //motors.setSpeeds(speed, speed);

    motors.setSpeeds(0, 0);
}



void moveBackwardDistance(float distance_mm){
    
    float mm_per_rev = PI * 32;
    int32_t targetCounts = -1* (distance_mm / mm_per_rev) * ENCODER_COUNTS_PER_REV;
    int32_t turnAngleIntegral = 0;
    float acceleration = 0;
    float currSpeed = 40;
    int32_t counter = 0;
    //distance_mm -= 60;

    encoders.getCountsAndResetLeft();
    encoders.getCountsAndResetRight();

    while (true){
        int32_t leftCount = encoders.getCountsLeft();
        int32_t rightCount = encoders.getCountsRight();
        int32_t averageCount =  (leftCount + rightCount) / 2;
        
        if (averageCount <= targetCounts){
            break;
        }

        turnSensorUpdate();

        turnAngleIntegral += -((int32_t)turnAngle/turnAngle1);
        int32_t turnSpeed = Kp * (-(int32_t)turnAngle / (turnAngle1)) + Kd*(-0.07 * turnRate) - Ki * (turnAngleIntegral);

        //turnSpeed = -1 * constrain(turnSpeed, -100, 100);
        turnSpeed = -1 * constrain(turnSpeed, -maxSpeed, maxSpeed);


        motors.setSpeeds(-1*((int32_t)maxSpeed-turnSpeed), -1* ((int32_t)maxSpeed+turnSpeed));

        display.clear();
        //display.println(-(int32_t)turnAngle/turnAngle1);

        display.println(averageCount);
        display.println(targetCounts);

    }

    motors.setSpeeds(0, 0);
}

void turn_right_90()
{
  // TODO: perhaps add trap motion profile
  //TODO: refine turn angles

  display.clear();
  encoders.getCountsAndResetLeft();
  encoders.getCountsAndResetRight();
  //turnSensorReset();
  motors.setSpeeds(0,0);
  float speed = 0;
  float turnAngleDegrees = 0;
  
  while (encoders.getCountsLeft() < 237 && encoders.getCountsRight() > -237){
    motors.setSpeeds(turn90Speed, -turn90Speed);
    turnSensorUpdate();
    display.clear();
    display.print(turnAngle/turnAngle1);
  }

  motors.setSpeeds(0,0);
  turnSensorUpdate();
  
  display.clear();
  display.print(turnAngle/turnAngle1);
  //turnAngle = turnAngle1*(turnAngle/turnAngle1 - 270);
  display.clear();
  display.print(-(int32_t)turnAngle/turnAngle1);

  turnSensorReset();

  //  while (true){
  // display.clear();
  // display.print(-(int32_t)turnAngle/turnAngle1);
  // turnSensorUpdate();
  // }
}

void turn_left_90()
{
  display.clear();
  encoders.getCountsAndResetLeft();
  encoders.getCountsAndResetRight();
  //turnSensorReset();
  motors.setSpeeds(0,0);

  display.print(-(int32_t)turnAngle/(float)turnAngle1);

  delay(500);
  
  while (encoders.getCountsRight() < 235 && encoders.getCountsLeft() > -235){
    motors.setSpeeds(-turn90Speed, turn90Speed);
    turnSensorUpdate();
  }

  motors.setSpeeds(0,0);
  turnSensorUpdate();
  
  display.clear();

  //turnAngle = 1 * (turnAngle90 -(int32_t)turnAngle);
  display.print(-(int32_t)turnAngle/float(turnAngle1));
  turnSensorReset();
  
  // while (true){
  // display.clear();
  // //display.gotoXY(0,1);
  // display.print(-(int32_t)turnAngle/float(turnAngle1));
  // turnSensorUpdate();
  // delay(5);
  // }


}


void new_turn_left_90()
{
  display.clear();
  encoders.getCountsAndResetLeft();
  encoders.getCountsAndResetRight();
  //turnSensorReset();
  motors.setSpeeds(0,0);
  float speed = 0;
  float relative_heading = 0;
  float turnAngleDegrees = 0;

  float time = millis();

  double Setpoint, Input, Output;
  //double Kp_turn = 5, Ki_turn = 2, Kd_turn = 1;
  //double Kp_turn = 3, Ki_turn = 1.5, Kd_turn = 0;
  double Kp_turn = 5, Ki_turn = 0.02, Kd_turn = 0;
  PID myPID(&Input, &Output, &Setpoint, Kp_turn, Ki_turn, Kd_turn, DIRECT);


  Setpoint = turnAngle90 / turnAngle1;
  myPID.SetMode(AUTOMATIC);
  myPID.SetOutputLimits(-maxSpeed, maxSpeed);

  while (millis() - time < 3000){

//PID control for turn
    Input = turnAngle / turnAngle1;
    myPID.Compute();
    // prevent outputs from -20 to 20
    if (Output > -20 && Output < 20) {
      if (Output < 0)
        Output -= 20;
      else
        Output += 20;
    }
    motors.setSpeeds(-Output, Output);
    turnSensorUpdate();
    display.clear();
    display.print(turnAngle / turnAngle1);
    display.gotoXY(0, 1);
    display.print(Output);
  }
  
  // while (abs(turnAngle90 - turnAngle) > 2 * turnAngle1){
  //   speed = 100*relative_heading/180;
  //   relative_heading = (turnAngle90 - turnAngle) /turnAngle1;
  //   Serial.println(relative_heading);
  //   if (speed < 0)
  //     speed -= 25;
  //   else
  //     speed += 25;

  //   // if heading is negative, turn right
  //   // if heading is positive, turn left

  //   // if (350>relative_heading > 180)
  //   //   motors.setSpeeds(speed, -speed);
  //   // else{
  //   //   motors.setSpeeds(-speed, speed);
  //   // }

  //   // FIX ABOVE
  //   motors.setSpeeds(-speed, speed);
  //   turnSensorUpdate();
  //   display.clear();
  //   display.print(turnAngle/turnAngle1);
  // }
  //delay(500);





  turnSensorUpdate();
    display.clear();
    display.print(turnAngle/turnAngle1);
  

  motors.setSpeeds(0,0);
  turnSensorUpdate();

  turnAngle = 1*(turnAngle90 -(int32_t)turnAngle);
  //turnSensorReset();
  
  display.clear();
  display.print(-(int32_t)turnAngle/(float)turnAngle1);
  //turnAngle = (turnAngle1*(90-(int32_t)turnAngle/turnAngle1));
  //turnAngle = (int32_t)(turnAngle90 -(int32_t)turnAngle)/float(turnAngle1)
  // while (true){
  // display.clear();
  // //turnAngle = turnAngle90 -(int32_t)turnAngle;
  // display.print(-(int32_t)turnAngle/(float)turnAngle1);
  // display.gotoXY(0,1);
  // display.print((turnAngle90 -(int32_t)turnAngle)/(float)turnAngle1);
  // turnSensorUpdate();
  // }

  //int32_t turnAngleNew = 90*turnAngle1 -(int32_t)turnAngle;
  //display.print(-(int32_t)turnAngle/turnAngle1);

  //turnSensorReset();
  
  while (true){
  display.clear();
  display.print(-(int32_t)turnAngle/float(turnAngle1));
  display.gotoXY(0,1);
  display.print((int32_t)(turnAngle90 -(int32_t)turnAngle)/float(turnAngle1));
  //display.print((int32_t)(turnAngle1*(90-(int32_t)turnAngle/turnAngle1))/turnAngle1);
  turnSensorUpdate();
  delay(5);
  }


}

void setup(){
  
  display.clear();
  display.print(F("Press B"));
  while(!buttonB.getSingleDebouncedPress());
  
  //delay(1000);
  turnSensorSetup();
  turnSensorReset();
  
  display.clear();
  display.print(F("Running..."));
}

void loop(){
  // TODO: see if the starting lag will be an issue
  int32_t curr_time = millis();
  //delay(500);
  if (!hasRun){

     moveForwardDistance(500);
     delay(500);
     turn_left_90();  
     delay(500);

     moveForwardDistance(500);
     delay(500);
     turn_left_90();  
     delay(500);

     moveForwardDistance(500);
     delay(500);
     turn_left_90();  
     delay(500);

     moveForwardDistance(500);
     delay(500);
     turn_left_90();  
     delay(500);

    hasRun = true;
  }

}

I also use the turnSensor.h file provided by pololu with a longer calibration time:

#include <stdint.h>
// Turnsensor.h provides functions for configuring the
// 3pi+ 32U4's gyro, calibrating it, and using it to
// measure how much the robot has turned about its Z axis.
//
// This file should be included *once* in your sketch,
// somewhere after you define objects named buttonA,
// display, and imu.

const int32_t calval = 5000;

#include <Wire.h>

// This constant represents a turn of 45 degrees.
const int32_t turnAngle45 = 0x20000000;

// This constant represents a turn of 90 degrees.
const int32_t turnAngle90 = turnAngle45 * 2;

// This constant represents a turn of approximately 1 degree.
const int32_t turnAngle1 = (turnAngle45 + 22) / 45;

/* turnAngle is a 32-bit unsigned integer representing the amount
the robot has turned since the last time turnSensorReset was
called.  This is computed solely using the Z axis of the gyro, so
it could be inaccurate if the robot is rotated about the X or Y
axes.

Our convention is that a value of 0x20000000 represents a 45
degree counter-clockwise rotation.  This means that a uint32_t
can represent any angle between 0 degrees and 360 degrees.  If
you cast it to a signed 32-bit integer by writing
(int32_t)turnAngle, that integer can represent any angle between
-180 degrees and 180 degrees. */
uint32_t turnAngle = 0;

// turnRate is the current angular rate of the gyro, in units of
// 0.07 degrees per second.
int16_t turnRate;

// This is the average reading obtained from the gyro's Z axis
// during calibration.
int16_t gyroOffset;

// This variable helps us keep track of how much time has passed
// between readings of the gyro.
uint16_t gyroLastUpdate = 0;


// This should be called to set the starting point for measuring
// a turn.  After calling this, turnAngle will be 0.
void turnSensorReset()
{
  gyroLastUpdate = micros();
  turnAngle = 0;
}

// Read the gyro and update the angle.  This should be called as
// frequently as possible while using the gyro to do turns.
void turnSensorUpdate()
{
  // Read the measurements from the gyro.
  imu.readGyro();
  turnRate = imu.g.z - gyroOffset;

  // Figure out how much time has passed since the last update (dt)
  uint16_t m = micros();
  uint16_t dt = m - gyroLastUpdate;
  gyroLastUpdate = m;

  // Multiply dt by turnRate in order to get an estimation of how
  // much the robot has turned since the last update.
  // (angular change = angular velocity * time)
  int32_t d = (int32_t)turnRate * dt;

  // The units of d are gyro digits times microseconds.  We need
  // to convert those to the units of turnAngle, where 2^29 units
  // represents 45 degrees.  The conversion from gyro digits to
  // degrees per second (dps) is determined by the sensitivity of
  // the gyro: 0.07 degrees per second per digit.
  //
  // (0.07 dps/digit) * (1/1000000 s/us) * (2^29/45 unit/degree)
  // = 14680064/17578125 unit/(digit*us)
  turnAngle += (int64_t)d * 14680064 / 17578125;
}

/* This should be called in setup() to enable and calibrate the
gyro.  It uses the display, yellow LED, and button A.  While the
display shows "Gyro cal", you should be careful to hold the robot
still.

The digital zero-rate level of the gyro can be as high as
25 degrees per second, and this calibration helps us correct for
that. */
void turnSensorSetup()
{
  Wire.begin();
  imu.init();
  imu.enableDefault();
  imu.configureForTurnSensing();

  display.clear();
  display.print(F("Gyro cal"));

  // Turn on the yellow LED in case the display is not available.
  ledYellow(1);

  // Delay to give the user time to remove their finger.
  delay(500);

  // Calibrate the gyro.
  int32_t total = 0;
  for (uint16_t i = 0; i < calval; i++)
  {
    // Wait for new data to be available, then read it.
    while(!imu.gyroDataReady()) {}
    imu.readGyro();

    // Add the Z axis reading to the total.
    total += imu.g.z;
  }
  ledYellow(0);
  gyroOffset = total / calval;

  // // Display the angle (in degrees from -180 to 180) until the
  // // user presses A.
  // display.clear();
  // turnSensorReset();
  // while (!buttonA.getSingleDebouncedRelease())
  // {
  //   turnSensorUpdate();
  //   display.gotoXY(0, 0);
  //   display.print((((int32_t)turnAngle >> 16) * 360) >> 16);
  //   display.print(F("   "));
  // }
  // display.clear();
}
12

After looking at the turnSensorUpdate function, I suspect inaccurate numerical integration of the gyro data.

This line looks like a major mistake in the Pololu code for that function, as micros() returns a 32 bit integer. I don’t see how the function ever worked properly.

  // Figure out how much time has passed since the last update (dt)
  uint16_t m = micros();

All variables associated with millis() and micros() must be declared unsigned long or uint32_t.

For accurate numerical integration, the time interval between integration steps should be fixed and as short as practical. It is not a good idea to mix display or other nonessential, time consuming activities with critical navigation functions.

  while (encoders.getCountsLeft() < 237 && encoders.getCountsRight() > -237){
    motors.setSpeeds(turn90Speed, -turn90Speed);
    turnSensorUpdate();
    display.clear();
    display.print(turnAngle/turnAngle1);
  }
13

Since all of the calculations involving m are done using 16-bit arithmetic (dt and gyroLastUpdate are also uint16_t), it works fine as long as we don’t wait more than 65 ms (65535 microseconds) between updates. However, using all 32-bit math would be a little more robust and the greater efficiency of the 16-bit math probably isn’t that significant. So, you could try changing it and seeing if it helps in your case, although at the point that you’re overflowing a 65 ms counter, that’s probably a longer integration period than you want for good accuracy (as Jim mentioned).

Brandon

14

I agree that with uint16_t intermediate variables, turnSensorUpdate() should work as expected, as long as less than 65 ms elapses between successive function calls. I imagine that these operations, especially the clear() could be the problem.

    display.clear();
    display.print(turnAngle/turnAngle1);

A time integration update period of 10 ms (100 Hz rate) generally seems accepted as a target figure for navigational position or orientation calculations, and I suspect that display.clear() alone takes quite a bit longer than 10 ms.

1 Like
15

Thank you for all of your help. The gyro is still somewhat inaccurate, but it seems perhaps better than before. Thanks again!

16

What did you change?

17

Just taking off the display.clear()/other display or nonessential statements. I’m not sure if it really made any difference, but it seems logical that using such code is not ideal.