Add PD controller to balance pendulum

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);