From 7ec186bee9bb9e908ce30fc888ad630928c68e1a Mon Sep 17 00:00:00 2001
From: Allanis <allanis@saracraft.net>
Date: Sat, 23 Mar 2013 21:19:11 +0000
Subject: [PATCH] [Add] Trying to work with a PID system for smart missiles.
---
src/physics.c | 37 +++++++++++++-----------
src/weapon.c | 78 +++++++++++++++++++++++++++++++++++++++++++++------
2 files changed, 89 insertions(+), 26 deletions(-)
diff --git a/src/physics.c b/src/physics.c
index b3e6c80..ee6050b 100644
--- a/src/physics.c
+++ b/src/physics.c
@@ -9,13 +9,15 @@
// MISC
// ================
double angle_diff(const double ref, double a) {
+ double d;
if(a < M_PI) a += 2*M_PI;
- double d = fmod((a-ref), 2*M_PI);
+ d = fmod((a-ref), 2*M_PI);
return (d <= M_PI) ? d : d - 2*M_PI;
}
void limit_speed(Vec2* vel, const double speed, const double dt) {
- double vmod = VMOD(*vel);
+ double vmod;
+ vmod = VMOD(*vel);
if(vmod > speed) // Should not go faster.
vect_pset(vel, (vmod-speed)*(1.-dt*3.) + speed, VANGLE(*vel));
}
@@ -94,15 +96,15 @@ static void simple_update(Solid* obj, const double dt) {
if(obj->dir < 0.) obj->dir += 2*M_PI;
double px, py, vx, vy;
- px = VX(obj->pos);
- py = VY(obj->pos);
- vx = VX(obj->vel);
- vy = VY(obj->vel);
+ px = obj->pos->x;
+ py = obj->pos->y;
+ vx = obj->vel->x;
+ vy = obj->vel->y;
if(obj->force.mod) { // Force applied on an object.
double ax, ay;
- ax = VX(obj->force)/obj->mass;
- ay = VY(obj->force)/obj->mass;
+ ax = obj->force->x/obj->mass;
+ ay = obj->force->y/obj->mass;
vx += ax*dt;
vy += ay*dt;
@@ -138,27 +140,27 @@ static void simple_update(Solid* obj, const double dt) {
#define RK4_MIN_H 0.01 // Minimal pass we want.
static void rk4_update(Solid* obj, const double dt) {
+ int i, N; // For iteration and pass calculation.
+ double h, px, py, vx, vy; // Pass and position/velocity values.
+ double ix, iy, tx, ty, ax, ay; // Initial and temp cartesian vector values.
// Make sure angle doesn't flip.
obj->dir += M_PI/180.*obj->dir_vel*dt;
if(obj->dir >= 2.*M_PI) obj->dir -= 2*M_PI;
else if(obj->dir < 0.) obj->dir += 2*M_PI;
- int N = (dt > RK4_MIN_H) ? (int)(dt/RK4_MIN_H) : 1;
- double h = dt / (double)N; // Step.
+ N = (dt > RK4_MIN_H) ? (int)(dt/RK4_MIN_H) : 1;
+ h = dt / (double)N; // Step.
- double px, py, vx, vy;
px = obj->pos.x;
py = obj->pos.y;
vx = obj->vel.x;
vy = obj->vel.y;
if(obj->force.mod) { // Force applied on object.
- int i;
- double ix, iy, tx, ty; // Initial and temp cartesian vector values.
-
- double ax, ay;
- ax = VX(obj->force)/obj->mass;
- ay = VY(obj->force)/obj->mass;
+ // Movement quantity theorem : m*a = \sum f.
+ ax = obj->force.x / obj->mass;
+ ay = obj->force.y / obj->mass;
+
for(i = 0; i < N; i++) {
// X component.
tx = ix = vx;
@@ -182,6 +184,7 @@ static void rk4_update(Solid* obj, const double dt) {
}
vect_cset(&obj->vel, vx, vy);
} else {
+ // Euler method -> p = v*t + 0.5*a*t^2 (no accel, so no error).
px += dt*vx;
py += dt*vy;
}
diff --git a/src/weapon.c b/src/weapon.c
index b7df6a0..2b645c7 100644
--- a/src/weapon.c
+++ b/src/weapon.c
@@ -18,6 +18,10 @@
#define VOICE_PRIORITY_BOLT 10 // Default.
#define VOICE_PRIORITY_AMMO 8 // Higher.
+// PID values.
+#define IMIN -50000
+#define IMAX 50000
+
// Some stuff from pilot.
extern Pilot** pilot_stack;
extern int pilots;
@@ -37,7 +41,10 @@ typedef struct Weapon_ {
// Update position and render.
void(*update)(struct Weapon_*, const double, WeaponLayer); // Position update and render.
- void(*think)(struct Weapon_*, const double); // Some missiles need to be inteligent.
+ void(*think)(struct Weapon_*, const double); // Some missiles need to be inteligent.a
+
+ double pid_last;
+ double pid_int;
} Weapon;
// Behind Pilot layer.
@@ -143,10 +150,43 @@ static void think_seeker(Weapon* w, const double dt) {
limit_speed(&w->solid->vel, w->outfit->u.amm.speed, dt);
}
+// ========================================================
// Smart seeker brain. Much better at homing.
+//
+// SYSTEM DYNAMICS
+// INPUT:
+// MV = missile velocity
+// RP = target relative position.
+//
+// Constants:
+// KE = error constant.
+// KF = face constant.
+//
+// INTERNAL VARIABLES:
+// FD = face dir.
+// E = error.
+// PID = PID
+//
+// OUTPUT:
+// T = turn modifier.
+//
+// -- Fancy diagram.
+//
+// MV + FD + T
+// ---->0----->[PID]----[KF]---+---->
+// -^ -^ |
+// RP | | E |
+// -----+ |------[KE]------
+//
+// ========================================================
static void think_smart(Weapon* w, const double dt) {
- double diff;
- Vec2 tv, sv;
+ Vec2 tv;
+
+ // Controler stuff.
+ double ke, kf; // Constants
+ double fd, e, ed, ei; // Internal variables.
+ double t; // Internal variables.
+ double kp, ki, kd; // PID.
if(w->target == w->parent) return; // No self shooting here.
@@ -158,12 +198,32 @@ static void think_smart(Weapon* w, const double dt) {
}
if(SDL_GetTicks() > (w->timer + w->outfit->u.amm.lockon)) {
- vect_cset(&tv, VX(p->solid->pos) + VX(p->solid->vel),
- VY(p->solid->pos) + VY(p->solid->vel));
- vect_cset(&sv, VX(w->solid->pos) + VX(w->solid->vel),
- VY(w->solid->pos) + VY(w->solid->vel));
- diff = angle_diff(w->solid->dir, vect_angle(&tv, &sv));
- w->solid->dir_vel = 10*diff*w->outfit->u.amm.turn; // Face the target.
+ // Begin controler.
+ // Constants.
+ kp = 1.;
+ kd = 1.;
+ ki = 1.;
+
+ // Calculate fd.
+ vect_cset(&tv, p->solid->pos.x - w->solid->pos.x,
+ p->solid->pos.y - w->solid->pos.y);
+ e = angle_diff(VANGLE(w->solid->vel), VANGLE(tv));
+
+ e = fd;
+ ed = fd - ke*w->pid_last;
+ w->pid_int += e;
+ if(w->pid_int > IMAX) w->pid_int = IMAX;
+ else if(w->pid_int < IMIN) w->pid_int = IMIN;
+ ei = w->pid_int;
+ t = e*kp + ed*kd + ei*ki;
+
+ // Final output.
+ t *= kf;
+ w->pid_last = t;
+
+ // End controller.
+
+ w->solid->dir_vel = t * w->outfit->u.amm.turn; // Face the target.
if(w->solid->dir_vel > w->outfit->u.amm.turn)
w->solid->dir_vel = w->outfit->u.amm.turn;
else if(w->solid->dir_vel < -w->outfit->u.amm.turn)