PID Control loop not stopping a motor

#1

I’ve ran into a problem with a pid loop where I can set the requested value in the debugger window and it works just fine. However when I connect my vex joystick to it it drives for ever in one direction, until I input the opposite direction for twice as long as it has been driving in that direction… the code I use is below


float  pid_Kp = 2.0;
float  pid_Ki = 0.04;
float  pid_Kd = 0.0;

#define PID_SENSOR_INDEX    I2C_1
#define PID_SENSOR_SCALE    1

#define PID_MOTOR_INDEX     port9
#define PID_MOTOR_SCALE     -1/2.5

#define PID_DRIVE_MAX       63
#define PID_DRIVE_MIN     (-63)

#define PID_INTEGRAL_LIMIT  50

static int   pidRunning = 1;
static float pidRequestedValue;

/*-----------------------------------------------------------------------------*/
/*                                                                             */
/*  pid control task                                                           */
/*                                                                             */
/*-----------------------------------------------------------------------------*/

task pidController()
{
	float  pidSensorCurrentValue;

	float  pidError;
	float  pidLastError;
	float  pidIntegral;
	float  pidDerivative;
	float  pidDrive;

	// If we are using an encoder then clear it
	if( SensorType PID_SENSOR_INDEX ] == sensorQuadEncoder )
		SensorValue PID_SENSOR_INDEX ] = 0;

	// Init the variables - thanks Glenn :)
	pidLastError  = 0;
	pidIntegral   = 0;

	while( true )
	{
		// Is PID control active ?
		if( pidRunning )
		{
			// Read the sensor value and scale
			pidSensorCurrentValue = SensorValue PID_SENSOR_INDEX ] * PID_SENSOR_SCALE;

			// calculate error
			pidError = pidSensorCurrentValue - pidRequestedValue;

			// integral - if Ki is not 0
			if( pid_Ki != 0 )
			{
				// If we are inside controlable window then integrate the error
				if( abs(pidError) < PID_INTEGRAL_LIMIT )
					pidIntegral = pidIntegral + pidError;
				else
					pidIntegral = 0;
			}
			else
				pidIntegral = 0;

			// calculate the derivative
			pidDerivative = pidError - pidLastError;
			pidLastError  = pidError;

			// calculate drive
			pidDrive = (pid_Kp * pidError) + (pid_Ki * pidIntegral) + (pid_Kd * pidDerivative);

			// limit drive
			if( pidDrive > PID_DRIVE_MAX )
				pidDrive = PID_DRIVE_MAX;
			if( pidDrive < PID_DRIVE_MIN )
				pidDrive = PID_DRIVE_MIN;

			// send to motor
			motor PID_MOTOR_INDEX] = pidDrive * PID_MOTOR_SCALE;
		}
		else
		{
			// clear all
			pidError      = 0;
			pidLastError  = 0;
			pidIntegral   = 0;
			pidDerivative = 0;
			motor PID_MOTOR_INDEX] = 0;
		}

		// Run at 50Hz
		wait1Msec( 25 );
	}
}
/*-----------------------------------------------------------------------------*/
/*                                                                                               */
/*  main task                                                                            */
/*                                                                                               */
/*-----------------------------------------------------------------------------*/

task main()
{
	// send the motor off somewhere


	nMotorEncoder[port9]= 0;
	// start the PID task
	
	startTask( pidController);	// use joystick to modify the requested position
	
	wait1Msec(35);


	while( true )
	{
while (SensorValue[dgtl1])//<---- is for a button so that if it is not pressed the code will not run.
{
		// maximum change for pidRequestedValue will be 127/4*20, around 640 counts per second
		// free spinning motor is 100rmp so 1.67 rotations per second
		// 1.67 * 360 counts is 600

		pidRequestedValue = pidRequestedValue + (vexRT(Ch1)/4);

}
		
		motor[port9] = 0;
}
wait1Msec(35);


}
#2

I believe that your issue is coming from where you set the variable pidRequestedValue, how your code is right now you only ever add the value of the controller, never subtract anything. This would cause the issues you pointed out, since the values of the joysticks range from -127 to 127. A common way that I use to avoid this issue in my code is to use something like this:

if(pidRequestedValue < (vexRT(ch1)/4))
{
pidRequestedValue = pidRequestedValue + (vexRT(ch1)/4);
}
else if(pidRequestedValue > (vexRT(ch1)/4))
{
pidRequestedValue = pidRequestedValue - (vexRT(ch1)/4);
}
else
{
pidRequestedValue = 0;
}

something like this would cause the variable to either be added or subtracted depending on if the variable is higher or lower than the controller, which should solve your issue

Hope this helps,
Chris

#3

If I am correct, adding a negative is the same as subtracting a positive, so whether the joystick is in a positive or negative position it should still add or subtract right?

Ex: 127 ±63.5 = 63.5
127 - 63.5 = 63.5

I also tried your solution, and it didn’t work, the motor would stop at a certain angle, and never go past it in the negative direction and the positive direction it just keep on going.

#4

Sounds right. That’s what the code says to do. Here is where you are confused:

What happens is that for as long as you’re holding Ch1 in one direction you keep adjusting pidRequestedValue every 35 ms. So, let’s say you start at 0 and you hold Ch1 at a value of 40 for 3.5 s. Then you’ve added 40/10=4 repeatedly 100 times. So now pidRequestedValue = 400. Now you move Ch1 to a value of -40. You’ll have to hold it there for 3.5 s just to add -4 repeatedly 100 times to get pidRequestedValue back to 0. Only then will you start getting negative values for pidRequestedValue.

#5

Looks like your code is a hybrid of code from here.
https://vexforum.com/t/a-pid-controller-in-robotc/20105/1
and possibly here
https://vexforum.com/t/simple-p-controller-for-arm-position/23699/1

The example code in the second thread would limit the value that pidRequestedValue could hold to an allowable range, in the example code it was between 400 and 300 as pidRequestedValue was directly correlated to values returned by a potentiometer.

#6

I understand that, but when the motor encoder hits ‘400’ the motor doesn’t stop, it continues forever in that direction. I Let it run for 10 minuets and it still didn’t stop moving.

(I did source the code from the first link)
(I not a great programmer =))

#7

I think you are really confused about what you’re doing. You’re setting the motor power to 400 (in this example) multiplied by your scale, which for your scale above gives you -160 (in this example), which is capped at -127 and so treated as -127. You’re not setting a target distance or something similar. So what you’re saying is that the motor doesn’t stop because you’ve told it to run at full speed and haven’t told it to stop yet.