Anson/Lander-Embedded
1
0
Fork 0
mirror of https://gitlab.com/lander-team/lander-cpp.git synced 2025-06-16 07:06:51 +00:00
Code Releases Activity

PID fully debugged

Browse Source
This commit is contained in:
bpmcgeeney 2021-10-14 17:39:17 -07:00
parent 591ddc77e4
commit af4a5d16d5
7 changed files with 159 additions and 117 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
include/Vehicle.h
View File

@ -35,6 +35,7 @@ struct Vehicle {
double yError, yPrevError;
double pError, pPrevError;
double i_yError, i_pError = 0;
double d_yError, d_pError;
double simTime;
int stepSize;

1
include/outVector.h
View File

@ -35,6 +35,7 @@ struct outVector {
std::vector<double> LQRy = std::vector<double>(length, 0.0);
std::vector<double> thrust = std::vector<double>(length, 0.0);
std::vector<double> d_yError = std::vector<double>(length, 0.0);
};
#endif

219
include/sim.h
View File

@ -3,11 +3,11 @@
void burnStartTimeCalc(struct Vehicle &);
void thrustSelection(struct Vehicle &, int t);
// void lqrCalc(struct Vehicle &);
void lqrCalc(struct Vehicle &);
void pidController(struct Vehicle &, struct Vehicle &);
void TVC(struct Vehicle &);
void vehicleDynamics(struct Vehicle &, struct Vehicle &, int t);
void state2vec(struct Vehicle &, struct outVector &, int t);
void state2vec(struct Vehicle &, struct Vehicle &, struct outVector &, int t);
void write2CSV(struct outVector &, struct Vehicle &);
double derivative(double x2, double x1, double dt);
double integral(double x2, double x1, double dt);
@ -28,11 +28,12 @@ bool sim(struct Vehicle &State, struct Vehicle &PrevState) {
// Start Sim
do {
thrustSelection(State, t);
pidController(State, PrevState); // lqrCalc(State);
TVC(State);
vehicleDynamics(State, PrevState, t);
state2vec(State, stateVector, t);
thrustSelection(State, t);
pidController(State, PrevState);
// lqrCalc(State);
TVC(State);
state2vec(State, PrevState, stateVector, t);
t += State.stepSize;
} while ((State.z > 0.0) || (State.thrust > 0.1));
@ -85,6 +86,84 @@ void burnStartTimeCalc(Vehicle &State) {
State.simTime = (State.burntime + burnStartTime) * 1000;
}
void vehicleDynamics(Vehicle &State, Vehicle &PrevState, int t) {
// Moment of Inertia
State.I11 = State.mass * ((1 / 12) * pow(State.vehicleHeight, 2) +
pow(State.vehicleRadius, 2) / 4);
State.I22 = State.mass * ((1 / 12) * pow(State.vehicleHeight, 2) +
pow(State.vehicleRadius, 2) / 4);
State.I33 = State.mass * 0.5 * pow(State.vehicleRadius, 2);
// Idot
if (t < 0.1) {
State.I11dot = 0;
State.I22dot = 0;
State.I33dot = 0;
State.x = 0;
State.y = 0;
State.ax = 0;
State.ay = 0;
State.az = State.Fz / State.massInitial;
} else {
State.I11dot = derivative(State.I11, PrevState.I11, State.stepSize);
State.I22dot = derivative(State.I22, PrevState.I22, State.stepSize);
State.I33dot = derivative(State.I33, PrevState.I33, State.stepSize);
// pdot, qdot, rdot
State.yawddot = (State.momentX - State.I11dot * PrevState.yawdot +
State.I22 * PrevState.pitchdot * PrevState.rolldot -
State.I33 * PrevState.pitchdot * PrevState.rolldot) /
State.I11;
State.pitchddot = (State.momentY - State.I22dot * PrevState.pitchdot -
State.I11 * PrevState.rolldot * PrevState.yawdot +
State.I33 * PrevState.rolldot * PrevState.yawdot) /
State.I22;
State.rollddot = (State.momentZ - State.I33dot * PrevState.rolldot +
State.I11 * PrevState.pitchdot * PrevState.yawdot -
State.I22 * PrevState.pitchdot * PrevState.yawdot) /
State.I33;
// p, q, r
State.yawdot = integral(State.yawddot, PrevState.yawdot, State.stepSize);
State.pitchdot =
integral(State.pitchddot, PrevState.pitchdot, State.stepSize);
State.rolldot = integral(State.rollddot, PrevState.rolldot, State.stepSize);
// Euler Angles
State.phidot =
State.yawdot + (sin(State.pitch) * (State.rolldot * cos(State.yaw) +
State.pitchdot * sin(State.yaw))) /
cos(State.pitch);
State.thetadot =
State.pitchdot * cos(State.yaw) - State.rolldot * sin(State.yaw);
State.psidot =
(State.rolldot * cos(State.yaw) + State.pitchdot * sin(State.yaw)) /
cos(State.pitch);
State.yaw = integral(State.phidot, PrevState.yaw, State.stepSize);
State.pitch = integral(State.thetadot, PrevState.pitch, State.stepSize);
State.roll = integral(State.psidot, PrevState.roll, State.stepSize);
// ax ay az
State.ax = (State.Fx / State.mass);
State.ay = (State.Fy / State.mass);
State.az = (State.Fz / State.mass);
// vx vy vz in Earth frame
State.vx = integral(State.ax, PrevState.vx, State.stepSize);
State.vy = integral(State.ay, PrevState.vy, State.stepSize);
State.vz = integral(State.az, PrevState.vz, State.stepSize);
// Xe
State.x = integral(State.vx, PrevState.x, State.stepSize);
State.y = integral(State.vy, PrevState.y, State.stepSize);
State.z = integral(State.vz, PrevState.z, State.stepSize);
}
}
void thrustSelection(Vehicle &State, int t) {
if (State.burnElapsed != 2000) {
@ -94,7 +173,7 @@ void thrustSelection(Vehicle &State, int t) {
State.mass = State.massInitial - (State.mdot * State.burnElapsed);
}
else if (abs(State.burnVelocity + State.vz) < .001) {
else if (abs(State.burnVelocity + State.vz) < 0.001) {
// Start burn
State.burnStart = t;
State.burnElapsed = 0;
@ -116,10 +195,12 @@ void thrustSelection(Vehicle &State, int t) {
174.43 * State.burnElapsed + 67.17;
else if ((State.burnElapsed > 3.382) && (State.burnElapsed < 3.46))
State.thrust = -195.78 * State.burnElapsed + 675.11;
State.thrust = -195.78 * State.burnElapsed - 675.11;
if (State.burnElapsed > 3.45)
if (State.burnElapsed > 3.45) {
State.thrustFiring = false;
State.thrust = 0;
}
}
void lqrCalc(Vehicle &State) {
@ -180,14 +261,6 @@ void lqrCalc(Vehicle &State) {
}
void pidController(Vehicle &State, struct Vehicle &PrevState) {
// Not used in this function, but it was in the LQR function so had to copy it
// over since we don't run LQR when running PID
State.I11 = State.mass * ((1 / 12) * pow(State.vehicleHeight, 2) +
pow(State.vehicleRadius, 2) / 4);
State.I22 = State.mass * ((1 / 12) * pow(State.vehicleHeight, 2) +
pow(State.vehicleRadius, 2) / 4);
State.I33 = State.mass * 0.5 * pow(State.vehicleRadius, 2);
// Make sure we start reacting when we start burning
if (State.thrust > 0.01) {
@ -199,17 +272,19 @@ void pidController(Vehicle &State, struct Vehicle &PrevState) {
State.i_pError = integral(State.pError, State.i_pError, State.stepSize);
// Derivative of Error
double d_yError =
derivative(State.yError, PrevState.yError, State.stepSize);
double d_pError =
derivative(State.pError, PrevState.pError, State.stepSize);
State.d_yError = derivative(State.yError, PrevState.yError, State.stepSize);
State.d_pError = derivative(State.pError, PrevState.pError, State.stepSize);
// PID Function - it says LQR but this is just so that it gets passed to the
// TVC block properly
State.LQRx = (State.Kp * State.yError + State.Ki * State.i_yError +
State.Kd * d_yError);
State.Kd * State.d_yError) /
State.momentArm;
State.LQRy = (State.Kp * State.pError + State.Ki * State.i_pError +
State.Kd * d_pError);
State.Kd * State.d_pError) /
State.momentArm;
} else {
State.LQRx = 0;
State.LQRy = 0;
@ -238,6 +313,7 @@ void TVC(Vehicle &State) {
State.momentX = 0;
State.momentY = 0;
State.momentZ = 0;
} else {
// Convert servo position to degrees for comparison to max allowable
State.xServoDegs = (180 / M_PI) * asin(State.LQRx / State.thrust);
@ -261,7 +337,7 @@ void TVC(Vehicle &State) {
State.Fx = State.thrust * sin(State.xServoDegs * (M_PI / 180));
State.Fy = State.thrust * sin(State.yServoDegs * (M_PI / 180));
State.Fz =
sqrt(pow(State.thrust, 2) - (pow(State.Fx, 2) + pow(State.Fy, 2))) +
sqrt(pow(State.thrust, 2) - pow(State.Fx, 2) - pow(State.Fy, 2)) +
(State.mass * g);
// Calculate moment created by Fx and Fy
@ -271,87 +347,8 @@ void TVC(Vehicle &State) {
}
}
void vehicleDynamics(Vehicle &State, Vehicle &PrevState, int t) {
// Idot
if (t < 0.1) {
State.I11dot = 0;
State.I22dot = 0;
State.I33dot = 0;
} else {
State.I11dot = derivative(State.I11, PrevState.I11, State.stepSize);
State.I22dot = derivative(State.I22, PrevState.I22, State.stepSize);
State.I33dot = derivative(State.I33, PrevState.I33, State.stepSize);
}
// pdot, qdot, rdot
State.yawddot = (State.momentX - State.I11dot * PrevState.yawdot +
State.I22 * PrevState.pitchdot * PrevState.rolldot -
State.I33 * PrevState.pitchdot * PrevState.rolldot) /
State.I11;
State.pitchddot = (State.momentY - State.I22dot * PrevState.pitchdot -
State.I11 * PrevState.rolldot * PrevState.yawdot +
State.I33 * PrevState.rolldot * PrevState.yawdot) /
State.I22;
State.rollddot = (State.momentZ - State.I33dot * PrevState.rolldot +
State.I11 * PrevState.pitchdot * PrevState.yawdot -
State.I22 * PrevState.pitchdot * PrevState.yawdot) /
State.I33;
if (t < 0.1) {
State.x = 0;
State.y = 0;
State.ax = 0;
State.ay = 0;
State.az = State.Fz / State.massInitial;
}
else {
// p, q, r
State.yawdot = integral(State.yawddot, PrevState.yawdot, State.stepSize);
State.pitchdot =
integral(State.pitchddot, PrevState.pitchdot, State.stepSize);
State.rolldot = integral(State.rollddot, PrevState.rolldot, State.stepSize);
// ax ay az
State.ax = (State.Fx / State.mass) +
(State.pitchdot * State.vz - State.rolldot * State.vy);
State.ay = (State.Fy / State.mass) +
(State.rolldot * State.vx - State.vz * State.yawdot);
State.az = (State.Fz / State.mass) +
(State.vy * State.yawdot - State.pitchdot * State.vx);
// vx vy vz in Earth frame
State.vx = integral(State.ax, PrevState.vx, State.stepSize);
State.vy = integral(State.ay, PrevState.vy, State.stepSize);
State.vz = integral(State.az, PrevState.vz, State.stepSize);
// Xe
State.x = integral(State.vx, PrevState.x, State.stepSize);
State.y = integral(State.vy, PrevState.y, State.stepSize);
State.z = integral(State.vz, PrevState.z, State.stepSize);
// Euler Angles
State.phidot =
State.yawdot + (sin(State.pitch) * (State.rolldot * cos(State.yaw) +
State.pitchdot * sin(State.yaw))) /
cos(State.pitch);
State.thetadot =
State.pitchdot * cos(State.yaw) - State.rolldot * sin(State.yaw);
State.psidot =
(State.rolldot * cos(State.yaw) + State.pitchdot * sin(State.yaw)) /
cos(State.pitch);
State.yaw = integral(State.phidot, PrevState.yaw, State.stepSize);
State.pitch = integral(State.thetadot, PrevState.pitch, State.stepSize);
State.roll = integral(State.psidot, PrevState.roll, State.stepSize);
}
// Set "prev" values for next timestep
PrevState = State;
}
void state2vec(Vehicle &State, outVector &stateVector, int t) {
void state2vec(Vehicle &State, Vehicle &PrevState, outVector &stateVector,
int t) {
stateVector.x[t] = State.x;
stateVector.y[t] = State.y;
stateVector.z[t] = State.z;
@ -380,6 +377,10 @@ void state2vec(Vehicle &State, outVector &stateVector, int t) {
stateVector.LQRx[t] = State.LQRx;
stateVector.LQRy[t] = State.LQRy;
stateVector.thrust[t] = State.thrust;
stateVector.d_yError[t] = State.d_yError;
// Set "prev" values for next timestep
PrevState = State;
}
void write2CSV(outVector &stateVector, Vehicle &State) {
@ -397,7 +398,8 @@ void write2CSV(outVector &stateVector, Vehicle &State) {
// Output file header. These are the variables that we output - useful for
// debugging
outfile << "t, x, y, z, vx, vy, vz, ax, ay, az, yaw, pitch, roll, yawdot, "
"pitchdot, rolldot, Servo1, Servo2, thrustFiring, LQRx, LQRy, thrust"
"pitchdot, rolldot, Servo1, Servo2, thrustFiring, LQRx, LQRy, "
"thrust, deriv"
<< std::endl;
std::cout << "Writing to csv...\n";
@ -433,7 +435,8 @@ void write2CSV(outVector &stateVector, Vehicle &State) {
outfile << stateVector.LQRx[t] << ", ";
outfile << stateVector.LQRy[t] << ", ";
outfile << stateVector.thrust[t] << std::endl;
outfile << stateVector.thrust[t] << ", ";
outfile << stateVector.d_yError[t] << std::endl;
}
outfile.close();
@ -441,7 +444,7 @@ void write2CSV(outVector &stateVector, Vehicle &State) {
}
double derivative(double current, double previous, double step) {
double dxdt = (previous - current) / (step / 1000);
double dxdt = (current - previous) / (step / 1000);
return dxdt;
}

14
input.csv
View File

@ -1,19 +1,19 @@
vx,0,m/s
vy,0,m/s
vz,0,m/s
yaw,45,degs
pitch,0,degs
yaw,75,degs
pitch,30,degs
roll,0,degs
yawdot,0,degs/s
pitchdot,0,degs/s
rolldot,0,degs/s
Max Servo Rotation,7.5,degs
Max Servo Rotation,7,degs
Initial Mass,1.2,kg
Propellant Mass,0.06,kg
Burn Time,3.3,s
Vehicle Height,0.53,m
Vehicle Radius,0.05,m
Moment Arm,0.15,m
Burn Time,3.302,s
Vehicle Height,0.5318,m
Vehicle Radius,0.05105,m
Moment Arm,0.145,m
Sim Step Size,1,ms
Kp,-6.8699,x
Ki,0,x

1 vx 0 m/s
2 vy 0 m/s
3 vz 0 m/s
4 yaw 45 75 degs
5 pitch 0 30 degs
6 roll 0 degs
7 yawdot 0 degs/s
8 pitchdot 0 degs/s
9 rolldot 0 degs/s
10 Max Servo Rotation 7.5 7 degs
11 Initial Mass 1.2 kg
12 Propellant Mass 0.06 kg
13 Burn Time 3.3 3.302 s
14 Vehicle Height 0.53 0.5318 m
15 Vehicle Radius 0.05 0.05105 m
16 Moment Arm 0.15 0.145 m
17 Sim Step Size 1 ms
18 Kp -6.8699 x
19 Ki 0 x

13
matlabHelpers/Untitled.asv Normal file
View File

@ -0,0 +1,13 @@
clear all; close all; clc;
syms I11 I22 I33 I11dot I22dot I33dot
I = [I11 0 0; 0 I22 0; 0 0 I33];
Idot = [I11dot 0 0; 0 I22dot 0; 0 0 I33dot];
syms yaw pitch roll yawdot pitchdot rolldot yawddot pitchddot rollddot
w =
I *

24
matlabHelpers/Untitled.m Normal file
View File

@ -0,0 +1,24 @@
clear all; close all; clc;
syms I11 I22 I33 I11dot I22dot I33dot
I = [I11 0 0; 0 I22 0; 0 0 I33];
Idot = [I11dot 0 0; 0 I22dot 0; 0 0 I33dot];
syms yaw pitch roll yawdot pitchdot rolldot yawddot pitchddot rollddot
w = [yawdot; pitchdot; rolldot];
Iw = I * w;
Idw = Idot * w;
C = cross(w, Iw);
syms momentX momentY momentZ
M = [momentX; momentY; momentZ];
A = M - Idw - C;
B = [A(1) A(2) A(3)];
X = B*inv(I);
E = [X(1); X(2); X(3)]

4
matlabHelpers/simPlot.m
View File

@ -27,8 +27,8 @@ rolldot = T(:, 16);
Servo1 = T(:, 17);
Servo2 = T(:, 18);
LQRx = T(:, 19);
LQRy = T(:, 20);
LQRx = T(:, 20);
LQRy = T(:, 21);
% Acceleration
subplot(3, 1, 1)