Skip to content

Instantly share code, notes, and snippets.

@sdaitzman

sdaitzman/PIDClass.cpp

Created November 7, 2019 12:48
Show Gist options

  • Select an option

    Learn more about clone URLs
  • Save sdaitzman/91c3ad7e2ab27e058901f66b3829aefe to your computer and use it in GitHub Desktop.

Select an option

Learn more about clone URLs
Save sdaitzman/91c3ad7e2ab27e058901f66b3829aefe to your computer and use it in GitHub Desktop.
Download ZIP

Revisions

  1. sdaitzman created this gist Nov 7, 2019.
    98 changes: 98 additions & 0 deletions PIDClass.cpp
    View file
    Original file line number Diff line number Diff line change
    @@ -0,0 +1,98 @@
    class PID_Controller {
    public:
    // initialize variables used in the loop
    float previousError = 0;
    float error = 0;

    float integral = 0;
    float derivative = 0;

    float output = 0;

    // tuning params
    float setPoint;
    float Kp;
    float Ki;
    float Kd;

    float wheelBase = 10; // cm

    unsigned long lastRun;

    // linear velocity
    float linVel = 50.0; // hey how fast is this?

    // initialization function for tuning PID
    PID_Controller(float setPoint, float Kp, float Ki, float Kd);

    // empty constructor for initialization
    void setPIDConstants(float setPoint, float Kp, float Ki, float Kd);

    // Defines a function that steps the motor speeds based on PID
    float loopStep(float leftSensor, float rightSensor, Speeds *motor_speed);
    };



    // ...



    PID_Controller::PID_Controller(float setPoint_new, float Kp_new, float Ki_new, float Kd_new) {
    // construct!
    // PID_Controller PID = PID_Controller(5.0, 6.0, 6.0, 6.0)

    setPoint = setPoint_new;
    Kp = Kp_new;
    Ki = Ki_new;
    Kd = Kd_new;
    lastRun = micros();
    };


    void PID_Controller::setPIDConstants(float setPoint_new, float Kp_new, float Ki_new, float Kd_new) {
    // change the PID constants used in the live loop

    setPoint = setPoint_new;
    Kp = Kp_new;
    Ki = Ki_new;
    Kd = Kd_new;
    }


    float PID_Controller::loopStep(float leftSensor, float rightSensor, Speeds *motor_speed) {
    // Takes in the current left and right sensor values every loop
    // Runs a step to update the motor speeds (speed) based on the error in the PID loop
    // output represented as an angular velocity

    // timing math
    unsigned long now = micros();
    unsigned long dt = now - lastRun;
    lastRun = now;

    // run the loop
    // error = setpoint - measured_value <- difference btw sensors
    error = setPoint - (leftSensor - rightSensor); // +- 1023
    // integral = integral + error * dt
    integral = integral + (error * dt);
    // derivative = (error - previous_error) / dt
    derivative = (error - previousError) / dt;
    // output = Kp * error + Ki * integral + Kd * derivative
    output = (Kp * error) + (Ki * integral) + (Kd * derivative);
    // previous_error = error
    previousError = error;

    // set speed with speed.left // speed.right = output;
    // idea: output of PID as angular velocity

    // Vl = s - (w d)/2
    // vr = s + (w d)/2
    // minimum of output: 0
    // maximum of output:
    motor_speed->left = linVel - (output * wheelBase) / 2;
    motor_speed->right = linVel + (output * wheelBase) / 2;

    return output;


    };