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