diff --git a/lib/Controller.cpp b/lib/Controller.cpp index 919ee7e..670c2e0 100644 --- a/lib/Controller.cpp +++ b/lib/Controller.cpp @@ -100,7 +100,7 @@ void FullController::FBControl() { int16_t FullController::pwmFunc(K_MAP consts, Errors errs) { if (oor) return 0; Constants constants = (errs.e < 0) ? consts.attracting : consts.repelling; - return (int)constrain(constants.K*(errs.e + constants.ki*errs.eInt + constants.kd*errs.eDiff), -(float)CAP,(float)CAP); + return (int)constrain(constants.kp*errs.e + constants.ki*errs.eInt + constants.kd*errs.eDiff, -(float)CAP,(float)CAP); } int16_t FullController::slewLimit(int16_t target, int16_t prev) { diff --git a/lib/Controller.hpp b/lib/Controller.hpp index eb5272d..e878577 100644 --- a/lib/Controller.hpp +++ b/lib/Controller.hpp @@ -17,7 +17,7 @@ #define CAP 200 typedef struct Constants { - float K; + float kp; float ki; float kd; } Constants; diff --git a/lib/PseudoSensorControl.cpp b/lib/PseudoSensorControl.cpp index b323bc0..3b68923 100644 --- a/lib/PseudoSensorControl.cpp +++ b/lib/PseudoSensorControl.cpp @@ -59,7 +59,7 @@ void PseudoSensorController::control() { int16_t PseudoSensorController::pwmFunc(K_MAP consts, Errors errs) { if (oor) return 0; Constants constants = (errs.e < 0) ? consts.attracting : consts.repelling; - return (int)constrain(constants.K*(errs.e + constants.ki*errs.eInt + constants.kd*errs.eDiff), -(float)CAP,(float)CAP); + return (int)constrain(constants.kp*errs.e + constants.ki*errs.eInt + constants.kd*errs.eDiff, -(float)CAP,(float)CAP); } int16_t PseudoSensorController::slewLimit(int16_t target, int16_t prev) { diff --git a/lib/PseudoSensorControl.hpp b/lib/PseudoSensorControl.hpp index 95ad9d2..457b58a 100644 --- a/lib/PseudoSensorControl.hpp +++ b/lib/PseudoSensorControl.hpp @@ -26,7 +26,7 @@ extern PinPair pinMap[4]; #define CAP 200 typedef struct Constants { - float K; + float kp; float ki; float kd; } Constants;