A PID controller in RobotC

[Team80_Giraffes]

@FullMetalMentor

Just curious: what do you mean by “reset the PID”? In jpearman’s code, I think he limits how much integral error builds up so the integral error does not “wind up” to some absurd value. Is there some way the integral error can be/should be reset to zero? perhaps when the arm is “close enough” to its target point?

Thanks.

Yes reset the integral to 0, especially when close to target. When you were last computing the integral, you may have been way far away from the target setting and it built up to a high value. If you are close in, the I will be too high and you will end up overshooting the target. Setting to 0 though will make it reset the PID.

Last error should be reset too to clear the D as well.

But if you can live with the overshoot, then it is optional. It should settle down eventually.

[TheMkrage]

Hey, I just had a question about using PID to keep the robot straight. To do this with encoders, I start a PID loop with the slower side and the aim and angle. And then I simultaniously start another PID loop with faster side’s encoder to the slower sides encoder. Is this right? Also, how do you keep the robot straight with only a gyro? If i start a PID loop with the gyro and the aim angle, then I can’t move forward at the same time. What is the solution for this? I know this is two questions, thank you.

[Team80_Giraffes]

@TheMkrage

Hey, I just had a question about using PID to keep the robot straight. To do this with encoders, I start a PID loop with the slower side and the aim and angle. And then I simultaniously start another PID loop with faster side’s encoder to the slower sides encoder. Is this right? Also, how do you keep the robot straight with only a gyro? If i start a PID loop with the gyro and the aim angle, then I can’t move forward at the same time. What is the solution for this? I know this is two questions, thank you.

Two ways to attack this. Assuming forward facing wheels.

1. Keep the gyro angle constant. Use PID to keep that set to the correct heading angle.

2. Keep the delta from left/right side to 0. Your PID manages the delta to 0.

They each have a slight drawback to be aware of. The gyro can drift slightly so what you think is your heading may change over time as the gyro drifts.

The left/right may occasionally have wheel slip which makes left right different. The sticky tires and foam tiles makes that less likely.

For each of these, based upon the error, you can be off a bit if you don’t keep track of how far off in the Y direction you have traveled. (forward is X direction) As you go off a bit to left, you correct right until same direction heading if using the gyro. However, you drifted left a little bit which make you off target (like getting a skyrise piece into the base or aligning to grab the next one). So you may want to go back that amount of drift versus just correcting the heading.

Holonomic drive changes this a bit to manage both X and Y and possibly angle as well with their own error calculations.

[ggurta]

@jpearman

There have been a couple of questions lately about PID control, in particular there is an open thread in the RobotC technical discussion but only Jesse can post answers there, so I thought it was time to show some code doing a simple implementation. I don’t claim to be an expert in this area, the math and implementation are pretty simple but tuning can be tricky and depends on the physical aspects of the system being controlled and it’s desired response.

There are some problems in implementing PID control on the cortex as the motor speed can only be updated at around 60Hz at best (ports 1 & 10) and in practice somewhat slower than that if using the other ports needing the motor controller. Commercial motor controllers would typically run at a higher frequency than this but that’s not an option for us.

The following code controls one motor and uses one encoder as feedback. A variable called “pidRequestedValue” can be set and the motor will run until the encoder count matches this value. The code allows joystick channel2 (right analog vertical) to change pidRequestedValue therefore causing the motor to run until the encoder count again matches.

The PID control runs as a task with a fixed wait time at the end of each pass through the loop. This gives us a constant delta time when comparing to the theoretical equations. As the derivative and integral terms are being calculated at a constant interval this delta time becomes irrelevant and is folded into the Kd and Ki constants.

The code has a bunch of definitions at the beginning used to set motor direction, encoder scaling (for example you could scale so the units were inches rather than raw counts), the maximum motor speeds and the limit for integrating the error.

#pragma config(Sensor, dgtl1,  myEncoder,        sensorQuadEncoder)
#pragma config(Motor,  port10, myMotor,    tmotorNormal, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

// PID using optical shaft encoder
//
// Shaft encoder has 360 pulses per revolution
//

#define PID_SENSOR_INDEX    myEncoder
#define PID_SENSOR_SCALE    1

#define PID_MOTOR_INDEX     myMotor
#define PID_MOTOR_SCALE     -1

#define PID_DRIVE_MAX       127
#define PID_DRIVE_MIN     (-127)

#define PID_INTEGRAL_LIMIT  50

// These could be constants but leaving
// as variables allows them to be modified in the debugger "live"
float  pid_Kp = 2.0;
float  pid_Ki = 0.04;
float  pid_Kd = 0.0;

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
    pidRequestedValue = 1000;

    // start the PID task
    StartTask( pidController );

    // use joystick to modify the requested position
    while( true )
        {
        // 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 Ch2 ]/4);
        
        wait1Msec(50);
        }

}

I have used this code in our robot, and am actively using robotC’s integrated PID. My team was wondering if there is any way to “turn off” this PID when we push a button? for example, we launch all our full court shots with the PID on, but can i push a button that will then turn off the PID and set the motors to a crude value of 50/127 lets say? Without using the slave motor function at all? i’m assuming i could add the slave function to the actual button if statement, but im not sure how to make sure that the IME is no longer controlling the motor value of even that one singular motor. To me it seems like if im not running the slave motor code, then the IME will adjust its speed, but my other motors will run at that set value of 50… any ideas on how to toggle this PId code on and off? thanks

[jpearman]

Four year old thread back from the dead.

Do you want to toggle PID in the original code I posted four years ago or the builtin PID that ROBOTC now has?

[ggurta]

@jpearman

Four year old thread back from the dead.

Do you want to toggle PID in the original code I posted four years ago or the builtin PID that ROBOTC now has?

Im sorry, i intended to use the thread that you recently posted about PID in robotC! i want to toggle it with the PID control RobotC has now

[jpearman]

@ggurta

Im sorry, i intended to use the thread that you recently posted about PID in robotC! i want to toggle it with the PID control RobotC has now

Then I will reply in the other thread.

[Inventor_Inventor]

Don’t encoders have 3600 ticks per revolution?

[jpearman]

@Inventor Inventor

Don’t encoders have 3600 ticks per revolution?

6 year old thread back from the dead, must be Halloween !

The Red quadrature encoders have 360 ticks per revolution.