PROS custom PID

I am attempting to write a custom PID controller where the left and right sides of the drivetrain have set points. It only uses inputs from 2 passive tracking wheels, one on the left and one on the right. There are 2 issues with the result. The first is that I cannot tune the gains to anything resembling what I want - it takes around 5 seconds to complete a small turn with my best settings. My second and main issue is that it usually behaves normally, but sometimes it goes crazy and drives a full speed into a wall or spins infinitely in the spot, This happens every 3 or so runs without changing the code in between. Here is the code:

#include "main.h"
#include "globals.h"

void initialize()
{
eleft.reset();
eright.reset();
emid.reset();
}


void disabled()
{

}


void competition_initialize()
{

}

double deltaTime;
double leftVel;
double rightVel;
int prevTime;
double prevLeftPos;
double prevRightPos;
double averageTrack;
double lproportional;
double lintegral;
double lderivative;
double rproportional;
double rintegral;
double rderivative;
double lerror;
double rerror;
double lout;
double rout;
double ple;
double pre;

int updateVelPID(double kP, double kI, double kD, int ltarget, int rtarget)
{
	deltaTime = pros::millis() - prevTime;
	leftVel = (eleft.get_value() - prevLeftPos) / deltaTime;
	rightVel = (eright.get_value() - prevRightPos) / deltaTime;
	prevLeftPos = eleft.get_value();
	prevRightPos = eright.get_value();
	prevTime = pros::millis();
	averageTrack = (leftVel + rightVel) / 2;
	lerror = ltarget - leftVel;
	rerror = rtarget - rightVel;

	lproportional = lerror;
	rproportional = rerror;

	lout = (lproportional * kP);
	rout = (rproportional * kP);


	BRD.move_voltage(rout);
	FRD.move_voltage(rout);
	BLD.move_voltage(lout);
	FLD.move_voltage(lout);


printf("left %f right %f \n", leftVel, rightVel);

	return 1;
}

int updatePosPID(double kP, double kI, double kD, int ltarget, int rtarget)
{
	deltaTime = pros::millis() - prevTime;
	leftVel = (eleft.get_value() - prevLeftPos) / deltaTime;
	rightVel = (eright.get_value() - prevRightPos) / deltaTime;
	prevLeftPos = eleft.get_value();
	prevRightPos = eright.get_value();
	prevTime = pros::millis();
	averageTrack = (leftVel + rightVel) / 2;
	ple = lerror;
	pre = rerror;
	lerror = ltarget - eleft.get_value();
	rerror = rtarget + eright.get_value();

	lproportional = lerror;
	rproportional = rerror;

if (abs(lintegral) < 5000)
{
	lintegral += lerror;
}

if (abs(rintegral) < 5000)
{
	rintegral += rerror;
}


	lderivative = lerror - ple;
	rderivative = rerror - pre;

	lout = (lproportional * kP) + (lintegral * kI) + (lderivative * kD);
	rout = (rproportional * kP) + (rintegral * kI) + (rderivative * kD);


	BRD.move_voltage(rout);
	FRD.move_voltage(rout);
	BLD.move_voltage(lout);
	FLD.move_voltage(lout);



printf("left %f right %f \n", lout, rout);

	return 1;
}

void balancePID(int left, int right)
{
	prevTime = pros::millis();
	lerror = 10;
	rerror = 10;
	int settle = 0;
	while(settle < 25)
	{
		if (abs(lerror) + abs(rerror) > 5)
		{
			settle = 0;
		}
		else
		{
			settle += 1;
		}
	updatePosPID(60, 0.5, 2, left, right);
//		master.print(0, 0, "left %f right %f \n", lout, rout);//std::to_string(lout));
	pros::delay(15);
	}
//	master.rumble(".");
	BRD.move_voltage(0);
	FRD.move_voltage(0);
	BLD.move_voltage(0);
	FLD.move_voltage(0);
}

void autonomous()
{
	balancePID(-200, 200);
	balancePID(0, 400);
}


void opcontrol()
{

int stickThresh = 10;

int bldpos = BLD.get_position();
int brdpos = BRD.get_position();
int fldpos = FLD.get_position();
int frdpos = FRD.get_position();

while(true)
{

	int leftStick = master.get_analog(ANALOG_LEFT_Y) * 2;
	int rightStick = master.get_analog(ANALOG_RIGHT_Y) * 2;

	int brake = 1;

	if(master.get_digital(DIGITAL_A))
	{

		FRD.set_brake_mode(pros::E_MOTOR_BRAKE_HOLD);
		BRD.set_brake_mode(pros::E_MOTOR_BRAKE_HOLD);
		FLD.set_brake_mode(pros::E_MOTOR_BRAKE_HOLD);
		BLD.set_brake_mode(pros::E_MOTOR_BRAKE_HOLD);

		pros::delay(50);

		FLD.move(200);
		FRD.move(200);
		BLD.move(200);
		BRD.move(200);

		pros::delay(100);

		FLD.move(0);
		FRD.move(0);
		BLD.move(0);
		BRD.move(0);

		brdpos = BRD.get_position();
		frdpos = FRD.get_position();
		bldpos = BLD.get_position();
		fldpos = FLD.get_position();

	}

	if (abs(leftStick) > stickThresh)
	{
		FRD.set_brake_mode(pros::E_MOTOR_BRAKE_COAST);
		BRD.set_brake_mode(pros::E_MOTOR_BRAKE_COAST);
		FLD.set_brake_mode(pros::E_MOTOR_BRAKE_COAST);
		BLD.set_brake_mode(pros::E_MOTOR_BRAKE_COAST);

		brake = 0;
		FLD.move_velocity(leftStick);
		BLD.move_velocity(leftStick);
		bldpos = BLD.get_position();
		fldpos = FLD.get_position();
	}
	else
	{
		if (brake == 0)
		{
		FLD.move_velocity(0);
		BLD.move_velocity(0);
		brake = 1;
		}
		else
		{
		FLD.move_absolute(fldpos, 200);
		BLD.move_absolute(bldpos, 200);
		}
	}

	if (abs(rightStick) > stickThresh)
	{
		FRD.set_brake_mode(pros::E_MOTOR_BRAKE_COAST);
		BRD.set_brake_mode(pros::E_MOTOR_BRAKE_COAST);
		FLD.set_brake_mode(pros::E_MOTOR_BRAKE_COAST);
		BLD.set_brake_mode(pros::E_MOTOR_BRAKE_COAST);

		brake = 0;
		FRD.move_velocity(rightStick);
		BRD.move_velocity(rightStick);
		brdpos = BRD.get_position();
		frdpos = FRD.get_position();
	}
	else
	{
		if (brake == 0)
		{
		FRD.move_velocity(0);
		BRD.move_velocity(0);
		brake = 1;
		}
		else
		{
		FRD.move_absolute(frdpos, 200);
		BRD.move_absolute(brdpos, 200);
		}

	}

	pros::delay(10);

}

}

How are you tuning your PID?

I recommend setting integral and derivative gains to 0, and setting the proportional gain aggressively.

Remember, any real world P loop is actually a PT(n) loop, which is why you even need the I and D controllers.

I’m also not too sure about your code. You should be compartmentalizing each controller into its own function, which should be called by a single ‘PID’ function that takes the current and desired values as inputs.

What may be happening here is that your delta time is out of sync with the actual elapsed time- either by delay or because it’s not correctly called.

For example:

deltaTime = pros::millis() - prevTime;
leftVel = (eleft.get_value() - prevLeftPos) / deltaTime;
rightVel = (eright.get_value() - prevRightPos) / deltaTime;
prevLeftPos = eleft.get_value
prevRightPos = eright.get_value();
prevTime = pros::millis();

Your right and left PID’s use the same delta time- despite the fact that these are computed sequentially. Additional time has elapsed between the calculation for delta and the calculation for the right PID.

Equally egregious is that you don’t redefine prevTime immediately after calculating deltaTime.

That should look like this:

current_time = pros::millis()
delta_time = cur_time-prev_time
prev_time = current_time

So those are two things you should fix:
1.) Correct the way and timing that prev_time and delta_time are calculated
2.) Compartmentalize your code: a single PID function with three subfunctions: P, I, D

Hope that helps!

3 Likes

This is what okapilib is for. It’s a set of libs on top of pros that implements all this stuff for you. You just have to set it up.
https://okapilib.github.io/OkapiLib/index.html

2 Likes