/ Mbed OS BagnoleAuto_PA_PAUL

.

Dependencies:   L432KC_SPI_Pey_Lal

asservissement.cpp

Committer:
voltxd
Date:
2022-05-18
Revision:
115:156b8234f2de
Parent:
113:2f8f255e99f2

File content as of revision 115:156b8234f2de:

#include "asservissement.hpp"
#include "toolbox.hpp"

double kp = 1;
double ki = 1;
double errorIntegral = 0;
double integralLimit = 10;

uint32_t PID(double error, double sampleRate)
{
    errorIntegral += error / sampleRate;
    errorIntegral = limitToInterval(errorIntegral, -integralLimit, integralLimit);
    double correction = kp * error + ki * errorIntegral;
    double correctionPWM = SPEED_TO_PWM_TRANSFORM(correction);
    return limitToInterval(correctionPWM, 1000, 1500);
}

void setPIDParameters(double kp_, double ki_)
{
    kp = kp_;
    ki = ki_;
}