PID Control loop not stopping a motor

  1. last week

    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. 6 days ago
    Edited 5 days ago by 9065_chris

    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. 3 days ago

    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. callen

    Oct 12 Braintree, MA, USA

    @WumblingBorkeys 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...

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

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

    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. jpearman

    Oct 12 Moderator, ROBOTC Tech Support, V5 Beta Moderator Los Angeles 8888
    Edited 3 days ago by jpearman

    Looks like your code is a hybrid of code from here.
    https://www.vexforum.com/index.php/6465-a-pid-controller-in-robotc/0
    and possibly here
    https://www.vexforum.com/index.php/9583-simple-p-controller-for-arm-position/0

    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.

 

or Sign Up to reply!