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

started debugging

Browse Source
This commit is contained in:
bpmcgeeney 2021-10-11 11:30:51 -07:00
parent 1b13a88c7d
commit 591ddc77e4
6 changed files with 59 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
include/Vehicle.h
View File

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

4
include/outVector.h
View File

@ -4,7 +4,7 @@
#define OUTVECTOR_H
struct outVector {
int length = 10000; // current sim runs ~5000 steps, x2 just in case
int length = 100000; // current sim runs ~5000 steps, x2 just in case
std::vector<double> x = std::vector<double>(length, 0.0);
std::vector<double> y = std::vector<double>(length, 0.0);
@ -33,6 +33,8 @@ struct outVector {
std::vector<double> LQRx = 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);
};
#endif

56
include/sim.h
View File

@ -14,7 +14,7 @@ double integral(double x2, double x1, double dt);
// Any parameters that are constants should be declared here instead of buried
// in code
double const dt = 0.001;
double const dt = 0.01;
double const g = -9.81;
bool sim(struct Vehicle &State, struct Vehicle &PrevState) {
@ -29,14 +29,13 @@ bool sim(struct Vehicle &State, struct Vehicle &PrevState) {
// Start Sim
do {
thrustSelection(State, t);
// lqrCalc(State);
pidController(State, PrevState);
pidController(State, PrevState); // lqrCalc(State);
TVC(State);
vehicleDynamics(State, PrevState, t);
state2vec(State, stateVector, t);
t++;
} while ((State.z > 0.0) || (State.thrust > 1.0));
t += State.stepSize;
} while ((State.z > 0.0) || (State.thrust > 0.1));
write2CSV(stateVector, State);
@ -59,7 +58,7 @@ void burnStartTimeCalc(Vehicle &State) {
double velocity = State.vz;
double h = 0;
double a, j, mass, thrust;
double mass, thrust;
// Piecewise functions for F15 thrust curve
for (double i = 0.148; i < 3.450; i = i + dt) {
@ -107,6 +106,7 @@ void thrustSelection(Vehicle &State, int t) {
if ((State.burnElapsed > 0.147) && (State.burnElapsed < 0.420)) {
State.thrustFiring = true;
State.thrust = 65.165 * State.burnElapsed - 2.3921;
} else if ((State.burnElapsed > 0.419) && (State.burnElapsed < 3.383))
State.thrust = 0.8932 * pow(State.burnElapsed, 6) -
11.609 * pow(State.burnElapsed, 5) +
@ -210,7 +210,6 @@ void pidController(Vehicle &State, struct Vehicle &PrevState) {
State.Kd * d_yError);
State.LQRy = (State.Kp * State.pError + State.Ki * State.i_pError +
State.Kd * d_pError);
} else {
State.LQRx = 0;
State.LQRy = 0;
@ -274,7 +273,7 @@ void TVC(Vehicle &State) {
void vehicleDynamics(Vehicle &State, Vehicle &PrevState, int t) {
// Idot
if (t < 1) {
if (t < 0.1) {
State.I11dot = 0;
State.I22dot = 0;
State.I33dot = 0;
@ -285,20 +284,20 @@ void vehicleDynamics(Vehicle &State, Vehicle &PrevState, int t) {
}
// pdot, qdot, rdot
State.yawddot = (State.momentX - State.I11dot * State.yawdot +
State.I22 * State.pitchdot * State.rolldot -
State.I33 * State.pitchdot * State.rolldot) /
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 * State.pitchdot -
State.I11 * State.rolldot * State.yawdot +
State.I33 * State.rolldot * State.yawdot) /
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 * State.rolldot +
State.I11 * State.pitchdot * State.yawdot -
State.I22 * State.pitchdot * State.yawdot) /
State.rollddot = (State.momentZ - State.I33dot * PrevState.rolldot +
State.I11 * PrevState.pitchdot * PrevState.yawdot -
State.I22 * PrevState.pitchdot * PrevState.yawdot) /
State.I33;
if (t < 1) {
if (t < 0.1) {
State.x = 0;
State.y = 0;
@ -322,7 +321,7 @@ void vehicleDynamics(Vehicle &State, Vehicle &PrevState, int t) {
State.az = (State.Fz / State.mass) +
(State.vy * State.yawdot - State.pitchdot * State.vx);
// vx vy vz in Body frame
// 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);
@ -333,13 +332,14 @@ void vehicleDynamics(Vehicle &State, Vehicle &PrevState, int t) {
State.z = integral(State.vz, PrevState.z, State.stepSize);
// Euler Angles
State.phidot = State.yawdot + (State.pitchdot * sin(State.yaw) +
State.rolldot * cos(State.yaw)) *
(sin(State.pitch) / cos(State.pitch));
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.pitch);
State.pitchdot * cos(State.yaw) - State.rolldot * sin(State.yaw);
State.psidot =
(State.pitchdot * sin(State.yaw) + State.rolldot * cos(State.yaw)) /
(State.rolldot * cos(State.yaw) + State.pitchdot * sin(State.yaw)) /
cos(State.pitch);
State.yaw = integral(State.phidot, PrevState.yaw, State.stepSize);
@ -379,6 +379,7 @@ void state2vec(Vehicle &State, outVector &stateVector, int t) {
stateVector.LQRx[t] = State.LQRx;
stateVector.LQRy[t] = State.LQRy;
stateVector.thrust[t] = State.thrust;
}
void write2CSV(outVector &stateVector, Vehicle &State) {
@ -396,13 +397,13 @@ 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"
"pitchdot, rolldot, Servo1, Servo2, thrustFiring, LQRx, LQRy, thrust"
<< std::endl;
std::cout << "Writing to csv...\n";
// writing to output file
for (int t = 0; t < State.simTime; t++) {
for (int t = 0; t < State.simTime; t += State.stepSize) {
outfile << t << ", ";
outfile << stateVector.x[t] << ", ";
@ -431,7 +432,8 @@ void write2CSV(outVector &stateVector, Vehicle &State) {
outfile << stateVector.thrustFiring[t] << ", ";
outfile << stateVector.LQRx[t] << ", ";
outfile << stateVector.LQRy[t] << std::endl;
outfile << stateVector.LQRy[t] << ", ";
outfile << stateVector.thrust[t] << std::endl;
}
outfile.close();

8
input.csv
View File

@ -1,7 +1,7 @@
vx,0,m/s
vy,0,m/s
vz,0,m/s
yaw,10,degs
yaw,45,degs
pitch,0,degs
roll,0,degs
yawdot,0,degs/s
@ -15,6 +15,6 @@ Vehicle Height,0.53,m
Vehicle Radius,0.05,m
Moment Arm,0.15,m
Sim Step Size,1,ms
Kp,-1,x
Ki,-1,x
Kd,-1,x
Kp,-6.8699,x
Ki,0,x
Kd,-0.775,x

1 vx 0 m/s
2 vy 0 m/s
3 vz 0 m/s
4 yaw 10 45 degs
5 pitch 0 degs
6 roll 0 degs
7 yawdot 0 degs/s
15 Vehicle Radius 0.05 m
16 Moment Arm 0.15 m
17 Sim Step Size 1 ms
18 Kp -1 -6.8699 x
19 Ki -1 0 x
20 Kd -1 -0.775 x

20
matlabHelpers/simPlot.m
View File

@ -27,6 +27,9 @@ rolldot = T(:, 16);
Servo1 = T(:, 17);
Servo2 = T(:, 18);
LQRx = T(:, 19);
LQRy = T(:, 20);
% Acceleration
subplot(3, 1, 1)
plot(t, az)
@ -91,3 +94,20 @@ title('Servo 2 Position vs Time')
xlabel('Time (ms)')
ylabel('Servo 2 Position (rad)')
%saveas(gcf,'outputs/Servo Position vs Time.png')
figure(4)
% Servo 1 Position
subplot(2, 1, 1)
plot(t, LQRx)
title('LQRx vs Time')
xlabel('Time (ms)')
ylabel('LQRx')
% Servo 2 Position
subplot(2, 1, 2)
plot(t, LQRy)
title('LQRy vs Time')
xlabel('Time (ms)')
ylabel('LQRy')
%saveas(gcf,'outputs/Servo Position vs Time.png')

3
src/main.cpp
View File

@ -75,6 +75,9 @@ int main() {
State.burnElapsed = 2000; // [s]
PrevState.thrust = 0; // [N]
PrevState = State;
std::cout << "START\n";
bool outcome = sim(State, PrevState);
std::cout << "Finished\n";