Speed control for 3pi+ robots


What I aim to do: Would like the 3pi+ to travel at a desirable speed to cover a straight path of 1 meters.

I have implemented a PID control loop for each motor as shown in the code below. I have tuned the PID weights for a heading velocity of 25 cm/s.

I was monitoring the encoder counts to verify if each wheel has travelled 1 meters at the end of the experiment. Based on the CPR (Counts Per Revolution) for these motors, I calculated the encoder counts, corresponding to 1 meter, to be 3561 counts (approximately).

So my thought process is that for the tuned PID gains at a desirable robot speed the encoder counts for each motor should be 3561 since the distance remains the same for all the experiments.
But what I noticed is different, for a desirable speed of 30 cm/s the robot travelled an additional 5.6cm that is 3860 counts.

I was hoping to understand why this is the case and any suggestions on this would be deeply appreciated. Attaching the code below.

  This program is intended to tune the PIDs for each of the wheel of the 3pi+ robots

#include <Pololu3piPlus32U4.h>

#define distance 1.0          //  distance travelled in meters
#define desired_speed 0.25    //  in m/s
#define top_speed 1.5         //  in m/s
#define CPR 358               //  counts per revolution
#define wheel_diameter 0.032  //  wheel diameter in meters
#define time_sampl 1          //  in milliseconds

using namespace Pololu3piPlus32U4;

LCD display;
Motors motors;
Encoders encoders;

// Left Wheel
#define P_l 0.395   
#define I_l 0.001  
#define D_l 0.0

// Right Wheel
#define P_r 0.365   
#define I_r 0.0     
#define D_r 0.0

// to store previous errors
float prev_err_l = 0;
float prev_err_r = 0;

// storing integral errors
int16_t int_err_vel_l = 0;
int16_t int_err_vel_r = 0;

// time variables
int start_time = 0;
float prev_time = 0.0;

// storing total number of encoder counts the robot has travelled
uint16_t total_enc_l = 0;
uint16_t total_enc_r = 0;
uint8_t k = 1;

int32_t int_error;

void setup() {
  // put your setup code here, to run once:

void loop() {
  // put your main code here, to run repeatedly:

  if (start_time == 0) 
    start_time = millis();

  float curr_time = (millis() - start_time) / 1000.0;

  // when time elapsed is more than the required time to travel 1.0 meters; stop program
  if (curr_time > distance / desired_speed) 
    // displaying encoder counts on the LCD 
    display.clear(); display.print(total_enc_l);    
    display.gotoXY(0, 1); display.print(total_enc_r);
    motors.setSpeeds(0, 0);
    delay(100000); //100 second delay
  if (millis() >= (prev_time * 1000 + time_sampl)) {
    int16_t curr_enc_l = encoders.getCountsAndResetLeft();
    int16_t curr_enc_r = encoders.getCountsAndResetRight();

    // intergral anti-windup
    if (int_err_vel_l > 1000) 
      int_err_vel_l = 0;
    if (int_err_vel_r > 1000) 
      int_err_vel_r = 0;

    // error calculations
    float err_vel_l = desired_speed - ((curr_enc_l * 0.1 / CPR) * 3.14 * wheel_diameter) / (curr_time - prev_time);
    float err_vel_r = desired_speed - ((curr_enc_r * 0.1 / CPR) * 3.14 * wheel_diameter) / (curr_time - prev_time);

    // differential error calculations
    int16_t diff_vel_l = (err_vel_l - prev_err_l) / (curr_time - prev_time);
    int16_t diff_vel_r = (err_vel_r - prev_err_r) / (curr_time - prev_time);

    // updating integral error summation
    int_err_vel_l += err_vel_l;
    int_err_vel_r += err_vel_r;

    // control wheel velocity calculations
    float ul = (float)(1.0 * desired_speed + (P_l * (err_vel_l) + I_l * (int_err_vel_l) + D_l * (diff_vel_l)));
    float ur = (float)(1.0 * desired_speed + (P_r * (err_vel_r) + I_r * (int_err_vel_r) + D_r * (diff_vel_r)));

    uint32_t cmd_l = (uint32_t)((ul*60.0) / (PI * wheel_diameter));  // in RPM
    uint32_t cmd_r = (uint32_t)((ur*60.0) / (PI * wheel_diameter));  // in RPM

    // sending control motor commands to motors
    motors.setSpeeds(cmd_l, cmd_r);
    // recording current state data for next iteration
    prev_err_l = err_vel_l;
    prev_err_r = err_vel_r;
    prev_time = curr_time;

    total_enc_l += curr_enc_l;
    total_enc_r += curr_enc_r; 


Update: I found an unusual observation today. Two days ago, I had tuned the PID gains for each of the motors for 25 cm/s with a steady state error of about 1cm. Today, when running the same experiment with no changes to the code I see a steady state error to be around 5cm. Requiring me to retune the PID gains.

I am having the robot run on a flex sheet, so naturally I expect some drift. But this is outright strange. Maybe I will have to see if it occurs again in the future.


Those results seem fairly accurate for using closed-loop speed control to control the position of the robot. There are a couple things that could cause error with this approach, but it generally comes down to inertia. If you are only relying on the desired speed and time to stop the robot after a certain distance (instead of using the encoder counts directly), then you are not taking into account the time it takes for the motor speed to ramp up to speed, and you are not considering how close you are to the target position so you can slow the motors down before they overshoot the goal. To account for those things, you might consider implementing a closed-loop position control algorithm on top of your speed control one.

As far as the error increasing when nothing is changing with your robot, I suspect that might be from increased wheel slippage as the tires get dirty. You might try cleaning them off (some isopropyl alcohol on a rag works well) and seeing if that makes it any better. Even with closed-loop position control, slippage is going to cause error, and I suspect most of it is probably coming from the initial movement of the robot, and while keeping the wheels clean should help, ultimately you might consider adding some acceleration limiting to minimize the chance of slipping.