Team Virgo v3 / Orion_newPCB_test_LV

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed-rtos mbed QEI BNO055 MPU6050_DMP_Nucleo-I2Cdev virgo3_imuHandler_Orion_PCB MAX17048 Servo

Fork of Orion_newPCB_test by Team Virgo v3

01_DriveTrain/pidControl.cpp

Committer:
akashvibhute
Date:
2016-01-18
Revision:
2:761e3c932ce0
Child:
6:690db8b5030b

File content as of revision 2:761e3c932ce0:

#include "pidControl.h"

pidControl::pidControl()
{
    for(int i=0; i<4; i++) PIDFf_gains[i]=0;
    for(int i=0; i<4; i++) {
        for(int j=0; j<2; i++)
            PIDFf_limits[i][j]=0;
    }
    for(int i=0; i<2; i++) output_limits[i]=0;
    for(int i=0; i<2; i++) controlError[i] =0;
    for(int i=0; i<4; i++) PIDFf_terms[i]=0;
    Summ_term=0;
}

void pidControl::setGains(float kP,float kI, float kD, float kFf)
{
    PIDFf_gains[0]=kP;
    PIDFf_gains[1]=kI;
    PIDFf_gains[2]=kD;
    PIDFf_gains[3]=kFf;
}

void pidControl::setTermLimits(float P_min, float P_max, float I_min, float I_max, float D_min, float D_max, float Ff_min, float Ff_max)
{
    PIDFf_limits[0][0] = P_min;
    PIDFf_limits[0][1] = P_max;

    PIDFf_limits[1][0] = I_min;
    PIDFf_limits[1][1] = I_max;

    PIDFf_limits[2][0] = D_min;
    PIDFf_limits[2][1] = D_max;

    PIDFf_limits[3][0] = Ff_min;
    PIDFf_limits[3][1] = Ff_max;
}

void pidControl::setOutputRange(float outMin, float outMax)
{
    output_limits[0]=outMin;
    output_limits[1]=outMax;
}

float pidControl::calcOutput(float setPoint, float feedback, float servoTime)
{
    controlError[1] = setPoint - feedback; //control error

    //proportional term 
    PIDFf_terms[0] = PIDFf_gains[0]*controlError[1];
    PIDFf_terms[0] = generalFunctions::constrain_f(PIDFf_terms[0], PIDFf_limits[0][0], PIDFf_limits[0][1]);

    
    //integral term 
    PIDFf_terms[1] += PIDFf_gains[1]*controlError[1]*servoTime;
    PIDFf_terms[1] = generalFunctions::constrain_f(PIDFf_terms[1], PIDFf_limits[1][0], PIDFf_limits[1][1]);

    //differential term
    PIDFf_terms[2] = PIDFf_gains[2] * (controlError[1] - controlError[0]) / servoTime;
    PIDFf_terms[2] = generalFunctions::constrain_f(PIDFf_terms[2], PIDFf_limits[2][0], PIDFf_limits[2][1]);
    
    //feedforward term
    PIDFf_terms[3] = PIDFf_gains[3]*setPoint;
    PIDFf_terms[3] = generalFunctions::constrain_f(PIDFf_terms[3], PIDFf_limits[3][0], PIDFf_limits[3][1]);
    
    //summ term
    Summ_term = PIDFf_terms[0] + PIDFf_terms[1] + PIDFf_terms[2] + PIDFf_terms[3];
    Summ_term = generalFunctions::constrain_f(Summ_term, output_limits[0], output_limits[1]);
    
    controlError[0] = controlError[1]; //store control error for usage in next iteration

    return(Summ_term);
}