Tustin Code

Lesson 31 - Enhancements to PID

Enhancements to PID

PID, while incredibly strong when tuned correctly, still has some weaknesses. For example, when the movement first starts, PID quite literally changes your motor power from 0 to 100 which can cause a whole bunch of problems with movement consistency. Furthermore, PID forward and backward movements will become very inconsistent if they are bumped by something and the robot’s heading strays off course. There are obviously many other weaknesses and potential improvements to PID but for the sake of this lesson we will be proposing solutions to these two issues. To summarize:


PID Issues:

  1. “Jerks” the motors into high power right away (this is due to the formula to calculate PID power).

  2. Cannot correct for heading error.



Enhancements to PID - Slew

To solve problem 1, we can implement “slew”. Slew essentially is limiting the rate of change of the speed of the robot per code loop. 


For example let’s say we set our slew rate to 5. That is to say, our speed can only increase by 5 per loop. At the start of the PID movement, rather than go from 0 to 100 power, the slew would then kick in and only make the robot go at 5 power. Then in the next iteration it would go to 10 power. Then 15 and so on and so on until the slew rate catches up to the PID. This would cause the robot to progressively accelerate rather than immediately jump at the start. A video of this can be seen here.


Using our previous PID example, slew could be implemented by doing this:

bool t_slew_on = true;
//asks for the movement target, the input sensor, the integral activation point, and the integral max
float calc (int target, float input, int integralKI, int maxI, float slew){
    prev_error = error; //stores previous error for calculating D
    error = target - input; //calculates error for calculating P and D

    if(std::abs(error) < integralKI){ //only lets I increment within a certain range of the target a certain range of the target
        integral += error;          
    }
    else{
        integral = 0;
    }
   
    if(integral >= 0){
        integral = std::min(integral, maxI) //keeps integral within a limit
    }
    else{
        integral = std::max(integral, -maxI);
    }

    derivative = error - prev_error; //calculates D

    power = t_kp*error + t_ki*integral + t_kd*derivative; //scales with constants to find power

    if(t_slew_on){
        if (std::abs(power) <= std::abs(prev_power) + std::abs(slew)){
            t_slew_on = false;
        }
        else{
            power = prev_power + slew;
        }
    }

    return power; //returns the calculated power
}


Enhancements to PID - Heading Correction


To solve problem 2, very simple IMU code can be added to the PID movement functions. The new code essentially modifies each chassis sides’ power proportional to how many degrees the heading is off course. It looks like: 


void forwardMove(int target){
    setConstants(0.39, 0.15, 0.01);

    float voltage;
    float encoder_average;
    int count = 0;
   
    float imu_start;

    reset_encoders(); //resets the encoders for each movement
    while(true){
       
        encoder_average = (leftDrive.get_position() + rightDrive.get_position()) / 2;
        voltage = calc(target, encoder_average, 200, 20, 1);
        heading_error = imu.get_rotation() - imu_offset;

        chas_move(voltage - heading_error, voltage + heading_error); //sets power to the chassis
        if (abs(target - encoder_average) <= 3) count++; //increases the counter when within 3 ticks
        if (count >= 28) break; //breaks out of while loop if approx 300 milliseconds within the 3 tick range
                                                      
        delay(10);
    }
    chas_move(0,0); //stops chassis after loop
}

VERY IMPORTANT NOTE: Some of the addition and subtraction signs may need to be flipped to work for your robot, experiment with the signs when you write code for your robot.