From e3c22d70487985a6dcb7a5866c8baeecbbaa4103 Mon Sep 17 00:00:00 2001 From: lonkaars Date: Sun, 14 May 2023 15:07:21 +0200 Subject: add PID controller --- zumo/pid.h | 7 +++++++ 1 file changed, 7 insertions(+) create mode 100644 zumo/pid.h (limited to 'zumo/pid.h') diff --git a/zumo/pid.h b/zumo/pid.h new file mode 100644 index 0000000..2cdbc91 --- /dev/null +++ b/zumo/pid.h @@ -0,0 +1,7 @@ +#pragma once + +#include "protocol.h" + +/** @brief edit `current` to be closer to `target` using PID controllers */ +void apply_pid(dui_state_t* target, dui_state_t* current); + -- cgit v1.2.3 From ff7a914055cf851c994ee037342a331902f84a0c Mon Sep 17 00:00:00 2001 From: lonkaars Date: Fri, 19 May 2023 13:34:31 +0200 Subject: add pidtest --- zumo/.gitignore | 1 + zumo/pid.cpp | 44 +++++++++++++++++++++----------------------- zumo/pid.h | 13 +++++++++++++ zumo/pidtest.cpp | 35 +++++++++++++++++++++++++++++++++++ zumo/pidtest.mk | 21 +++++++++++++++++++++ 5 files changed, 91 insertions(+), 23 deletions(-) create mode 100644 zumo/pidtest.cpp create mode 100644 zumo/pidtest.mk (limited to 'zumo/pid.h') 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; } diff --git a/zumo/pid.h b/zumo/pid.h index 2cdbc91..fbcd063 100644 --- a/zumo/pid.h +++ b/zumo/pid.h @@ -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 +#include + +#include "pid.h" + +std::random_device rd; +std::mt19937 rng(rd()); +std::uniform_real_distribution 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) + -- cgit v1.2.3 From 473ab3fd794d0baddd9a6a29ab25cea3d2217fc8 Mon Sep 17 00:00:00 2001 From: lonkaars Date: Fri, 19 May 2023 14:46:06 +0200 Subject: pid tuned --- zumo/pid.cpp | 27 +++++++++++++++++++++++---- zumo/pid.h | 9 +++++---- zumo/pidtest.cpp | 35 +++++++++-------------------------- 3 files changed, 37 insertions(+), 34 deletions(-) (limited to 'zumo/pid.h') diff --git a/zumo/pid.cpp b/zumo/pid.cpp index 9bef08f..fabe1da 100644 --- a/zumo/pid.cpp +++ b/zumo/pid.cpp @@ -1,9 +1,14 @@ #include "pid.h" PID::PID(float P, float I, float D) { - A0 = P + I*dt + D/dt; - A1 = -P - 2*D/dt; - A2 = D/dt; + A0 = P + I * dt; + A1 = -P; + A0d = D / dt; + A1d = -2 * D / dt; + A2d = D / dt; + tau = D / (P * N); + alpha = dt / (2 * tau); + reset(0.0); } // https://en.wikipedia.org/wiki/PID_controller#Pseudocode @@ -11,7 +16,17 @@ 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]; + + output = output + A0 * error[0] + A1 * error[1]; + + d1 = d0; + d0 = A0d * error[0] + A1d * error[1] + A2d * error[2]; + fd1 = fd0; + fd0 = ((alpha) / (alpha + 1)) * (d0 + d1) - ((alpha - 1) / (alpha + 1)) * fd1; + output = output + fd0; + + if (output < -1) output = -1; + if (output > 1) output = 1; return output; } @@ -19,6 +34,10 @@ void PID::reset(float value) { error[0] = 0.0; error[1] = 0.0; error[2] = 0.0; + d0 = 0; + d1 = 0; + fd0 = 0; + fd1 = 0; output = value; } diff --git a/zumo/pid.h b/zumo/pid.h index fbcd063..ef733d6 100644 --- a/zumo/pid.h +++ b/zumo/pid.h @@ -4,10 +4,11 @@ class PID { private: - float A0, A1, A2; - float error[3] = {0}; - float dt = 0.010; - float output = 0; + float A0, A1, A0d, A1d, A2d, tau, alpha, d0, d1, fd0, fd1; + float error[3]; + float output; + const float dt = 1.0; + const float N = 10.0; public: PID(float P, float I, float D); diff --git a/zumo/pidtest.cpp b/zumo/pidtest.cpp index f5198bb..b9ce50b 100644 --- a/zumo/pidtest.cpp +++ b/zumo/pidtest.cpp @@ -3,33 +3,16 @@ #include "pid.h" -std::random_device rd; -std::mt19937 rng(rd()); -std::uniform_real_distribution 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); + P = -0.02; + I = 0.13; + D = -300; + 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); + fprintf(stderr, "P: %.3f :: I: %.3f :: D: %.3f\n", P, I, D); + for (unsigned int i = 0; i < 100; i++) { + printf("%2.8f\n", test.iter(i < 50 ? 1.0 : 0.0)); + } } -- cgit v1.2.3 From ba026d8229744a01818d38552ec7271e689d19eb Mon Sep 17 00:00:00 2001 From: lonkaars Date: Fri, 19 May 2023 14:50:22 +0200 Subject: update pid in arduino sketch --- zumo/pid.cpp | 9 ++++----- zumo/pid.h | 2 +- 2 files changed, 5 insertions(+), 6 deletions(-) (limited to 'zumo/pid.h') diff --git a/zumo/pid.cpp b/zumo/pid.cpp index fabe1da..7c1cbda 100644 --- a/zumo/pid.cpp +++ b/zumo/pid.cpp @@ -41,13 +41,12 @@ void PID::reset(float value) { 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); +PID speed_pid = PID(); +PID steer_pid = PID(); +PID speed_mod_pid = PID(); void apply_pid(dui_state_t* target, dui_state_t* current) { 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; + current->speed_mod = speed_mod_pid.iter(target->speed_mod); } diff --git a/zumo/pid.h b/zumo/pid.h index ef733d6..beb73ac 100644 --- a/zumo/pid.h +++ b/zumo/pid.h @@ -11,7 +11,7 @@ private: const float N = 10.0; public: - PID(float P, float I, float D); + PID(float P = -0.02, float I = 0.13, float D = -300.0); float iter(float target); void reset(float value); }; -- cgit v1.2.3