Anson/Lander-Embedded
1
0
Fork 0
mirror of https://gitlab.com/lander-team/lander-cpp.git synced 2025-06-16 15:17:23 +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 yError, yPrevError;
double pError, pPrevError; double pError, pPrevError;
double i_yError, i_pError = 0; double i_yError, i_pError = 0;
double d_yError, d_pError;
double simTime; double simTime;
int stepSize; 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> LQRy = std::vector<double>(length, 0.0);
std::vector<double> thrust = 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 #endif

219
include/sim.h
View File

@ -3,11 +3,11 @@
void burnStartTimeCalc(struct Vehicle &); void burnStartTimeCalc(struct Vehicle &);
void thrustSelection(struct Vehicle &, int t); void thrustSelection(struct Vehicle &, int t);
// void lqrCalc(struct Vehicle &); void lqrCalc(struct Vehicle &);
void pidController(struct Vehicle &, struct Vehicle &); void pidController(struct Vehicle &, struct Vehicle &);
void TVC(struct Vehicle &); void TVC(struct Vehicle &);
void vehicleDynamics(struct Vehicle &, struct Vehicle &, int t); 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 &); void write2CSV(struct outVector &, struct Vehicle &);
double derivative(double x2, double x1, double dt); double derivative(double x2, double x1, double dt);
double integral(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 // Start Sim
do { do {
thrustSelection(State, t);
pidController(State, PrevState); // lqrCalc(State);
TVC(State);
vehicleDynamics(State, PrevState, t); 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; t += State.stepSize;
} while ((State.z > 0.0) || (State.thrust > 0.1)); } while ((State.z > 0.0) || (State.thrust > 0.1));
@ -85,6 +86,84 @@ void burnStartTimeCalc(Vehicle &State) {
State.simTime = (State.burntime + burnStartTime) * 1000; 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) { void thrustSelection(Vehicle &State, int t) {
if (State.burnElapsed != 2000) { if (State.burnElapsed != 2000) {
@ -94,7 +173,7 @@ void thrustSelection(Vehicle &State, int t) {
State.mass = State.massInitial - (State.mdot * State.burnElapsed); 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 // Start burn
State.burnStart = t; State.burnStart = t;
State.burnElapsed = 0; State.burnElapsed = 0;
@ -116,10 +195,12 @@ void thrustSelection(Vehicle &State, int t) {
174.43 * State.burnElapsed + 67.17; 174.43 * State.burnElapsed + 67.17;
else if ((State.burnElapsed > 3.382) && (State.burnElapsed < 3.46)) 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.thrustFiring = false;
State.thrust = 0;
}
} }
void lqrCalc(Vehicle &State) { void lqrCalc(Vehicle &State) {
@ -180,14 +261,6 @@ void lqrCalc(Vehicle &State) {
} }
void pidController(Vehicle &State, struct Vehicle &PrevState) { 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 // Make sure we start reacting when we start burning
if (State.thrust > 0.01) { 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); State.i_pError = integral(State.pError, State.i_pError, State.stepSize);
// Derivative of Error // Derivative of Error
double d_yError = State.d_yError = derivative(State.yError, PrevState.yError, State.stepSize);
derivative(State.yError, PrevState.yError, State.stepSize); State.d_pError = derivative(State.pError, PrevState.pError, State.stepSize);
double 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 // PID Function - it says LQR but this is just so that it gets passed to the
// TVC block properly // TVC block properly
State.LQRx = (State.Kp * State.yError + State.Ki * State.i_yError + 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.LQRy = (State.Kp * State.pError + State.Ki * State.i_pError +
State.Kd * d_pError); State.Kd * State.d_pError) /
State.momentArm;
} else { } else {
State.LQRx = 0; State.LQRx = 0;
State.LQRy = 0; State.LQRy = 0;
@ -238,6 +313,7 @@ void TVC(Vehicle &State) {
State.momentX = 0; State.momentX = 0;
State.momentY = 0; State.momentY = 0;
State.momentZ = 0; State.momentZ = 0;
} else { } else {
// Convert servo position to degrees for comparison to max allowable // Convert servo position to degrees for comparison to max allowable
State.xServoDegs = (180 / M_PI) * asin(State.LQRx / State.thrust); 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.Fx = State.thrust * sin(State.xServoDegs * (M_PI / 180));
State.Fy = State.thrust * sin(State.yServoDegs * (M_PI / 180)); State.Fy = State.thrust * sin(State.yServoDegs * (M_PI / 180));
State.Fz = 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); (State.mass * g);
// Calculate moment created by Fx and Fy // Calculate moment created by Fx and Fy
@ -271,87 +347,8 @@ void TVC(Vehicle &State) {
} }
} }
void vehicleDynamics(Vehicle &State, Vehicle &PrevState, int t) { void state2vec(Vehicle &State, Vehicle &PrevState, outVector &stateVector,
// Idot int t) {
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) {
stateVector.x[t] = State.x; stateVector.x[t] = State.x;
stateVector.y[t] = State.y; stateVector.y[t] = State.y;
stateVector.z[t] = State.z; stateVector.z[t] = State.z;
@ -380,6 +377,10 @@ void state2vec(Vehicle &State, outVector &stateVector, int t) {
stateVector.LQRx[t] = State.LQRx; stateVector.LQRx[t] = State.LQRx;
stateVector.LQRy[t] = State.LQRy; stateVector.LQRy[t] = State.LQRy;
stateVector.thrust[t] = State.thrust; 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) { 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 // Output file header. These are the variables that we output - useful for
// debugging // debugging
outfile << "t, x, y, z, vx, vy, vz, ax, ay, az, yaw, pitch, roll, yawdot, " 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::endl;
std::cout << "Writing to csv...\n"; std::cout << "Writing to csv...\n";
@ -433,7 +435,8 @@ void write2CSV(outVector &stateVector, Vehicle &State) {
outfile << stateVector.LQRx[t] << ", "; outfile << stateVector.LQRx[t] << ", ";
outfile << stateVector.LQRy[t] << ", "; outfile << stateVector.LQRy[t] << ", ";
outfile << stateVector.thrust[t] << std::endl; outfile << stateVector.thrust[t] << ", ";
outfile << stateVector.d_yError[t] << std::endl;
} }
outfile.close(); outfile.close();
@ -441,7 +444,7 @@ void write2CSV(outVector &stateVector, Vehicle &State) {
} }
double derivative(double current, double previous, double step) { double derivative(double current, double previous, double step) {
double dxdt = (previous - current) / (step / 1000); double dxdt = (current - previous) / (step / 1000);
return dxdt; return dxdt;
} }

14
input.csv
View File

@ -1,19 +1,19 @@
vx,0,m/s vx,0,m/s
vy,0,m/s vy,0,m/s
vz,0,m/s vz,0,m/s
yaw,45,degs yaw,75,degs
pitch,0,degs pitch,30,degs
roll,0,degs roll,0,degs
yawdot,0,degs/s yawdot,0,degs/s
pitchdot,0,degs/s pitchdot,0,degs/s
rolldot,0,degs/s rolldot,0,degs/s
Max Servo Rotation,7.5,degs Max Servo Rotation,7,degs
Initial Mass,1.2,kg Initial Mass,1.2,kg
Propellant Mass,0.06,kg Propellant Mass,0.06,kg
Burn Time,3.3,s Burn Time,3.302,s
Vehicle Height,0.53,m Vehicle Height,0.5318,m
Vehicle Radius,0.05,m Vehicle Radius,0.05105,m
Moment Arm,0.15,m Moment Arm,0.145,m
Sim Step Size,1,ms Sim Step Size,1,ms
Kp,-6.8699,x Kp,-6.8699,x
Ki,0,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); Servo1 = T(:, 17);
Servo2 = T(:, 18); Servo2 = T(:, 18);
LQRx = T(:, 19); LQRx = T(:, 20);
LQRy = T(:, 20); LQRy = T(:, 21);
% Acceleration % Acceleration
subplot(3, 1, 1) subplot(3, 1, 1)