.
Dependencies: L432KC_SPI_Pey_Lal
asservissement.cpp@113:2f8f255e99f2, 2022-05-17 (annotated)
- Committer:
- voltxd
- Date:
- Tue May 17 22:44:42 2022 +0000
- Revision:
- 113:2f8f255e99f2
- Child:
- 115:156b8234f2de
.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
voltxd | 113:2f8f255e99f2 | 1 | #include "asservissement.hpp" |
voltxd | 113:2f8f255e99f2 | 2 | #include "toolbox.hpp" |
voltxd | 113:2f8f255e99f2 | 3 | |
voltxd | 113:2f8f255e99f2 | 4 | double kp = 1; |
voltxd | 113:2f8f255e99f2 | 5 | double ki = 1; |
voltxd | 113:2f8f255e99f2 | 6 | double errorIntegral = 0; |
voltxd | 113:2f8f255e99f2 | 7 | double integralLimit = 10; |
voltxd | 113:2f8f255e99f2 | 8 | |
voltxd | 113:2f8f255e99f2 | 9 | uint32_t PID(double error, double sampleRate) |
voltxd | 113:2f8f255e99f2 | 10 | { |
voltxd | 113:2f8f255e99f2 | 11 | errorIntegral += error / sampleRate; |
voltxd | 113:2f8f255e99f2 | 12 | errorIntegral = limitToInterval(errorIntegral, -integralLimit, integralLimit); |
voltxd | 113:2f8f255e99f2 | 13 | double correction = kp * error + ki * errorIntegral; |
voltxd | 113:2f8f255e99f2 | 14 | double correctionPWM = SPEED_TO_PWM_TRANSFORM(correction); |
voltxd | 113:2f8f255e99f2 | 15 | return limitToInterval(correctionPWM, 1000, 1500); |
voltxd | 113:2f8f255e99f2 | 16 | } |