diff options
| -rw-r--r-- | zumo/.gitignore | 1 | ||||
| -rw-r--r-- | zumo/pid.cpp | 44 | ||||
| -rw-r--r-- | zumo/pid.h | 13 | ||||
| -rw-r--r-- | zumo/pidtest.cpp | 35 | ||||
| -rw-r--r-- | zumo/pidtest.mk | 21 |
5 files changed, 91 insertions, 23 deletions
diff --git a/zumo/.gitignore b/zumo/.gitignore index d3e1535..888e905 100644 --- a/zumo/.gitignore +++ b/zumo/.gitignore @@ -3,3 +3,4 @@ main *.hex compile_commands.json .cache +pidtest diff --git a/zumo/pid.cpp b/zumo/pid.cpp index 594d136..9bef08f 100644 --- a/zumo/pid.cpp +++ b/zumo/pid.cpp @@ -1,35 +1,33 @@ #include "pid.h" -class PID { -private: - float A0, A1, A2; - float error[3] = {0}; - float dt = 0.010; - float output = 0; +PID::PID(float P, float I, float D) { + A0 = P + I*dt + D/dt; + A1 = -P - 2*D/dt; + A2 = D/dt; +} -public: - PID(float P, float I, float D) { - A0 = P + I*dt + D/dt; - A1 = -P - 2*D/dt; - A2 = D/dt; - } +// https://en.wikipedia.org/wiki/PID_controller#Pseudocode +float PID::iter(float target) { + error[2] = error[1]; + error[1] = error[0]; + error[0] = target - output; + output = output + A0 * error[0] + A1 * error[1] + A2 * error[2]; + return output; +} - // https://en.wikipedia.org/wiki/PID_controller#Pseudocode - float iter(float current, float target) { - error[2] = error[1]; - error[1] = error[0]; - error[0] = target - current; - output = output + A0 * error[0] + A1 * error[1] + A2 * error[2]; - return output; - } -}; +void PID::reset(float value) { + error[0] = 0.0; + error[1] = 0.0; + error[2] = 0.0; + output = value; +} PID speed_pid(0.1, 0.0, 0.0); // TODO: tune these (garbage values) PID steer_pid(0.1, 0.0, 0.0); PID speed_mod_pid(1, 1, 1); void apply_pid(dui_state_t* target, dui_state_t* current) { - current->speed = speed_pid.iter(current->speed, target->speed); - current->steer = steer_pid.iter(current->steer, target->steer); + current->speed = speed_pid.iter(target->speed); + current->steer = steer_pid.iter(target->steer); // current->speed_mod = speed_mod_pid.iter(current->speed_mod, target->speed_mod); current->speed_mod = target->speed_mod; } @@ -2,6 +2,19 @@ #include "protocol.h" +class PID { +private: + float A0, A1, A2; + float error[3] = {0}; + float dt = 0.010; + float output = 0; + +public: + PID(float P, float I, float D); + float iter(float target); + void reset(float value); +}; + /** @brief edit `current` to be closer to `target` using PID controllers */ void apply_pid(dui_state_t* target, dui_state_t* current); diff --git a/zumo/pidtest.cpp b/zumo/pidtest.cpp new file mode 100644 index 0000000..f5198bb --- /dev/null +++ b/zumo/pidtest.cpp @@ -0,0 +1,35 @@ +#include <cstdio> +#include <random> + +#include "pid.h" + +std::random_device rd; +std::mt19937 rng(rd()); +std::uniform_real_distribution<float> uni(0,13); + +auto random_integer = uni(rng); + +int main() { + float P, I, D; + do { + // P = uni(rng); + // I = uni(rng); + // D = uni(rng); + P = 10; + I = 0.1; + D = 10; + PID test(P, I, D); + test.reset(0.0); + + float val = 0; + for (unsigned int i = 0; i < 100; i++) val = test.iter(1.0); + // if (val > 0.999 && val < 1.001) { + fprintf(stderr, "P: %.3f :: I: %.3f :: D: %.3f\n", P, I, D); + test.reset(0.0); + for (unsigned int i = 0; i < 100; i++) { + printf("%2.8f\n", test.iter(1.0)); + } + exit(0); + // } + } while (false); +} diff --git a/zumo/pidtest.mk b/zumo/pidtest.mk new file mode 100644 index 0000000..5ffe1e1 --- /dev/null +++ b/zumo/pidtest.mk @@ -0,0 +1,21 @@ +CPP = g++ +LD = g++ +RM = rm -f +CFLAGS = +LFLAGS = +TARGET = pidtest + +SRCS := pidtest.cpp pid.cpp +OBJS := pidtest.o pid.o + +all: pidtest + +%.o: %.cpp + $(CPP) -c $(CFLAGS) $< -o $@ + +$(TARGET): $(OBJS) + $(LD) $^ $(LFLAGS) -o $@ + +clean: + $(RM) $(TARGET) $(OBJS) + |