.
Dependencies: L432KC_SPI_Pey_Lal
Diff: asservissement.cpp
- Revision:
- 113:2f8f255e99f2
- Child:
- 115:156b8234f2de
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/asservissement.cpp Tue May 17 22:44:42 2022 +0000
@@ -0,0 +1,16 @@
+#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);
+}
\ No newline at end of file