diff --git a/main.c b/main.c
index 35ff4f2..65ff301 100644
--- a/main.c
+++ b/main.c
@@ -13,9 +13,17 @@
 #define CART_WIDTH 50
 #define CART_HEIGHT 20
 #define MASS_RADIUS 10
+#define SETPOINT_LN_LEN 40
 
-#define A_KP 60.0
-#define A_KD 20.0
+#define A_KP 80.0
+#define A_KD 10.0
+
+#define S_KP 0.08
+#define S_KD 0.08
+
+#define MIN(x, y) ((x) < (y) ? (x) : (y))
+#define MAX(x, y) ((x) > (y) ? (x) : (y))
+#define CLAMP(x, min, max) MAX(MIN(x, max), min)
 
 typedef struct {
 	double s, ps; // Cart displacement
@@ -51,19 +59,24 @@ 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 control(const state_t *s, double pos, double dt)
 {
-	static double prev_a = 0.0;
+	static double prev_s = 0.0, prev_a = 0.0;
 
-	const double err_a = s->a;
+	const double err_s = pos - s->s;
+	const double deriv_s = (err_s - prev_s) / dt;
+	const double target_a = CLAMP(S_KP * err_s + S_KD * deriv_s, -0.3, 0.3);
+	prev_s = err_s;
+
+	const double err_a = s->a - target_a;
 	const double deriv_a = (err_a - prev_a) / dt;
-	const double out = A_KP * err_a + A_KD * deriv_a;
+	const double out = CLAMP(A_KP * err_a + A_KD * deriv_a, -5.0, 5.0);
 	prev_a = err_a;
 
 	return out;
 }
 
-static void render(SDL_Renderer *r, const state_t *s)
+static void render(SDL_Renderer *r, const state_t *s, double pos)
 {
 	static const double sf = SCENE_WIDTH;
 	static const double sc_l = sf * l;
@@ -104,6 +117,12 @@ static void render(SDL_Renderer *r, const state_t *s)
 		.h = 2 * MASS_RADIUS,
 	};
 	SDL_RenderFillRect(r, &m);
+
+	// Position setpoint
+	const int sc_setpoint = sc_anchor.x + (int)(sf * pos);
+	SDL_RenderDrawLine(
+	    r, sc_setpoint, WIN_HEIGHT - MARGIN_Y, sc_setpoint,
+	    WIN_HEIGHT - MARGIN_Y - SETPOINT_LN_LEN);
 }
 
 int main(void)
@@ -125,6 +144,7 @@ int main(void)
 	const double dt = 1.0 / mode.refresh_rate;
 
 	state_t state = { .a = 0.1 };
+	double pos = 0.0;
 	while (1) {
 		SDL_Event e;
 		while (SDL_PollEvent(&e)) {
@@ -132,13 +152,13 @@ int main(void)
 				goto quit;
 		}
 
-		const double k = control(&state, dt);
+		const double k = control(&state, pos, dt);
 		update(&state, dt, k);
 
 		SDL_SetRenderDrawColor(renderer, 255, 255, 255, 255);
 		SDL_RenderClear(renderer);
 		SDL_SetRenderDrawColor(renderer, 0, 0, 0, 255);
-		render(renderer, &state);
+		render(renderer, &state, pos);
 
 		SDL_RenderPresent(renderer);
 	}