PID Control Help

VEX V5 Technical Support UNOFFICIAL Tech Support
#1

Hi everyone! Hope you all had a great Christmas and happy early new year too! So after reading several guides on PID programming, I seen to have figured out the proportional and derivative parts and they work together perfectly in a PD control code; however, it is when I incorporate the integral part of the code that things start malfunctioning. Here’s what my code currently looks like:


#pragma config(Sensor, dgtl1,  DriveQuadR,     sensorQuadEncoder)
#pragma config(Sensor, dgtl3,  DriveQuadL,     sensorQuadEncoder)
#pragma config(Motor,  port1,           FrontMotorR,   tmotorVex393_HBridge, openLoop, driveRight)
#pragma config(Motor,  port2,           BackMotorR,    tmotorVex393_MC29, openLoop, driveRight)
#pragma config(Motor,  port3,           FrontMotorL,   tmotorVex393_MC29, openLoop, reversed, driveLeft)
#pragma config(Motor,  port4,           BackMotorL,    tmotorVex393_MC29, openLoop, reversed, driveLeft)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

float pid_Kp = 0.5;	//1.2
float pid_Ki = 0.05;	//1.05
float pid_Kd = 0.55;	//0.0

void pidControlDrive(int pidRequestedValue)
{
	float pidErrorL;
	float pidErrorR;
	float pidLastErrorL;
	float pidLastErrorR;
	float pidCurrentSensorValueL;
	float pidCurrentSensorValueR;
	float pidDriveL;
	float pidDriveR;
	float pidIntegralL;
	float pidIntegralR;
	float pidDerivativeL;
	float pidDerivativeR;

	SensorValue[DriveQuadL] = 0;
	SensorValue[DriveQuadR] = 0;

	pidLastErrorL = 0;
	pidLastErrorR = 0;
	pidIntegralL = 0;
	pidIntegralR = 0;

	while(true)
	{
		pidCurrentSensorValueL = SensorValue[DriveQuadL];
		pidCurrentSensorValueR = SensorValue[DriveQuadR];

		//Proportional Part
		pidErrorL = pidCurrentSensorValueL - pidRequestedValue;
		pidErrorR = pidCurrentSensorValueR - pidRequestedValue;

		//Integral Part
		if(pid_Ki != 0)
		{
			if(((abs(pidErrorL) + abs(pidErrorR)) / 2) < 50)
			{
				pidIntegralL = pidIntegralL + pidErrorL;
				pidIntegralR = pidIntegralR + pidErrorR;
			}
			else
			{
				pidIntegralL = 0;
				pidIntegralR = 0;
			}
		}
		else
		{
   		pidIntegralL = 0;
   		pidIntegralR = 0;
   	}

		//Derivative Part
		pidDerivativeL = pidErrorL - pidLastErrorL;
    pidLastErrorL  = pidErrorL;
    pidDerivativeR = pidErrorR - pidLastErrorR;
    pidLastErrorR  = pidErrorL;

    //Calculating the motor speed
		pidDriveL = (pid_Kp * pidErrorL) + (pid_Ki * pidIntegralL) + (pid_Kd * pidLastErrorL);
		pidDriveR = (pid_Kp * pidErrorR) + (pid_Ki * pidIntegralR) + (pid_Kd * pidLastErrorR);

		motor[FrontMotorL] = pidDriveL;
		motor[BackMotorL] = pidDriveL;
		motor[FrontMotorR] = pidDriveR;
		motor[BackMotorR] = pidDriveR;
	}
}

task main()
{
	pidControlDrive(1500);
}

I mainly used jpearman’s post as a foundation for my code along with a few changes. I am currently using the VEX Skyrise field in Virtual Worlds with the FantasticBot to test my code. Now my main problem with this is that after I include the integral part and set the requested value to be at 1500 or any number really, the robot goes to the value fine but once it reaches it, the wheels become sporadic in moving forward and back. In addition to this, I have another question on how I used the code to correspond to a left and right side of the robot in order to make it move straight. Would this be an efficient way of achieving that or would there be a better way. I remember someone mentioning a “master and slave reading” thing but did not really understand it. Thanks for taking the time to read and hope you guys can help!

#2

The robot keeps bouncing back and forth once you reach your setpoint because you need to cap the Integral error. Include a conditional statement that keeps your total integral error from going over 10 (and under -10) and your motor will simply hold at that value instead of bouncing back and forth at too high of a value.

#3

Hm okay that seemed to work. Thanks! I guess 50 was too high of a limit. How do you figure out the correct limit? Is it just guess and check?

1 Like
#4

As for the base sync for master and slave, your gain is essentially our master’s encoder no. of ticks minus the slave’s. From there on your pid works somewhat the same way as what you did. Assuming that you moving forward, you will add the final PID value to your slave(let the power that u give it to be 127), and minus 127 from the final PID value for your master.

If you want to you can always change the given power, from 127 to the pid controller that control to a target point.

1 Like
#5