cdo/cart-and-pole
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Add additional PD controller for cart position

Browse Source
This commit is contained in:
Camden Dixie O'Brien 2025-05-24 10:06:55 +01:00
parent 6d12b84e02
commit 9d31085e80
1 changed files with 29 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
main.c
View File

@ -13,9 +13,17 @@
#define CART_WIDTH 50 #define CART_WIDTH 50
#define CART_HEIGHT 20 #define CART_HEIGHT 20
#define MASS_RADIUS 10 #define MASS_RADIUS 10
#define SETPOINT_LN_LEN 40
#define A_KP 60.0 #define A_KP 80.0
#define A_KD 20.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 { typedef struct {
double s, ps; // Cart displacement double s, ps; // Cart displacement
@ -51,19 +59,24 @@ static void update(state_t *s, double dt, double k)
s->pa += dt * pa_dot; 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 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; prev_a = err_a;
return out; 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 sf = SCENE_WIDTH;
static const double sc_l = sf * l; 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, .h = 2 * MASS_RADIUS,
}; };
SDL_RenderFillRect(r, &m); 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) int main(void)
@ -125,6 +144,7 @@ int main(void)
const double dt = 1.0 / mode.refresh_rate; const double dt = 1.0 / mode.refresh_rate;
state_t state = { .a = 0.1 }; state_t state = { .a = 0.1 };
double pos = 0.0;
while (1) { while (1) {
SDL_Event e; SDL_Event e;
while (SDL_PollEvent(&e)) { while (SDL_PollEvent(&e)) {
@ -132,13 +152,13 @@ int main(void)
goto quit; goto quit;
} }
const double k = control(&state, dt); const double k = control(&state, pos, dt);
update(&state, dt, k); update(&state, dt, k);
SDL_SetRenderDrawColor(renderer, 255, 255, 255, 255); SDL_SetRenderDrawColor(renderer, 255, 255, 255, 255);
SDL_RenderClear(renderer); SDL_RenderClear(renderer);
SDL_SetRenderDrawColor(renderer, 0, 0, 0, 255); SDL_SetRenderDrawColor(renderer, 0, 0, 0, 255);
render(renderer, &state); render(renderer, &state, pos);
SDL_RenderPresent(renderer); SDL_RenderPresent(renderer);
} }