Hello
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:
display.clear();
delay(1000);
}
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
return;
}
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;
}
}
Thanks
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.