From 6d12b84e02f1dc76332514d1ede092809ab30dca Mon Sep 17 00:00:00 2001
From: Camden Dixie O'Brien <cobrien@witekio.com>
Date: Sat, 24 May 2025 09:18:34 +0100
Subject: [PATCH] Add PD controller to balance pendulum

---
 main.c | 18 +++++++++++++++++-
 1 file changed, 17 insertions(+), 1 deletion(-)

diff --git a/main.c b/main.c
index 77eda05..35ff4f2 100644
--- a/main.c
+++ b/main.c
@@ -14,6 +14,9 @@
 #define CART_HEIGHT 20
 #define MASS_RADIUS 10
 
+#define A_KP 60.0
+#define A_KD 20.0
+
 typedef struct {
 	double s, ps; // Cart displacement
 	double a, pa; // Pendulum angle (from vertical)
@@ -48,6 +51,18 @@ static void update(state_t *s, double dt, double k)
 	s->pa += dt * pa_dot;
 }
 
+static double control(const state_t *s, double dt)
+{
+	static double prev_a = 0.0;
+
+	const double err_a = s->a;
+	const double deriv_a = (err_a - prev_a) / dt;
+	const double out = A_KP * err_a + A_KD * deriv_a;
+	prev_a = err_a;
+
+	return out;
+}
+
 static void render(SDL_Renderer *r, const state_t *s)
 {
 	static const double sf = SCENE_WIDTH;
@@ -117,7 +132,8 @@ int main(void)
 				goto quit;
 		}
 
-		update(&state, dt, 0.0);
+		const double k = control(&state, dt);
+		update(&state, dt, k);
 
 		SDL_SetRenderDrawColor(renderer, 255, 255, 255, 255);
 		SDL_RenderClear(renderer);