Anson/Lander-Embedded
1
0
Fork 0
mirror of https://gitlab.com/lander-team/lander-cpp.git synced 2025-07-23 14:41:25 +00:00
Code Releases Activity

Resolved #3

Browse Source
This commit is contained in:
bpmcgeeney
2021-09-17 12:53:59 -07:00
parent dbb6888602
commit 61a82eb579
12 changed files with 10139 additions and 5746 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
include/Vehicle.h
View File

@@ -1,7 +1,7 @@
#include <array>
#ifndef SVARS_H
#define SVARS_H
#ifndef VEHICLE_H
#define VEHICLE_H
struct Vehicle {
double x, y, z;
@@ -33,4 +33,4 @@ struct Vehicle {
int stepSize;
};
#endif
#endif

34
include/outVector.h Normal file
View File

@@ -0,0 +1,34 @@
#include <vector>
#ifndef OUTVECTOR_H
#define OUTVECTOR_H
struct outVector {
int length = 10000; // 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);
std::vector<double> z = std::vector<double>(length, 0.0);
std::vector<double> vx = std::vector<double>(length, 0.0);
std::vector<double> vy = std::vector<double>(length, 0.0);
std::vector<double> vz = std::vector<double>(length, 0.0);
std::vector<double> ax = std::vector<double>(length, 0.0);
std::vector<double> ay = std::vector<double>(length, 0.0);
std::vector<double> az = std::vector<double>(length, 0.0);
std::vector<double> yaw = std::vector<double>(length, 0.0);
std::vector<double> pitch = std::vector<double>(length, 0.0);
std::vector<double> roll = std::vector<double>(length, 0.0);
std::vector<double> yawdot = std::vector<double>(length, 0.0);
std::vector<double> pitchdot = std::vector<double>(length, 0.0);
std::vector<double> rolldot = std::vector<double>(length, 0.0);
std::vector<double> servo1 = std::vector<double>(length, 0.0);
std::vector<double> servo2 = std::vector<double>(length, 0.0);
};
#endif

185
include/sim.h
View File

@@ -1,11 +1,13 @@
#include "Vehicle.h"
#include "outVector.h"
void burnStartTimeCalc(struct Vehicle &);
void thrustSelection(struct Vehicle &, int t);
void lqrCalc(struct Vehicle &);
void TVC(struct Vehicle &);
void vehicleDynamics(struct Vehicle &, struct Vehicle &, int t);
void write2CSV(struct Vehicle &, std::fstream &outfile, int t);
void state2vec(struct Vehicle &, struct outVector &, int t);
void write2CSV(struct outVector &);
double derivative(double x2, double x1, double dt);
double integral(double x2, double x1, double dt);
@@ -16,32 +18,11 @@ double const g = -9.81;
bool sim(struct Vehicle &State, struct Vehicle &PrevState) {
// defining a few random values here cause I'm lazy
State.burnElapsed = 2000;
State.mass = State.massInitial;
PrevState.thrust = 0;
outVector stateVector;
// Determine when to burn
burnStartTimeCalc(State);
// Deleting any previous output file
if (remove("simOut.csv") != 0)
perror("Error deleting file");
else
puts("File successfully deleted");
// Define and open output file "simOut.csv"
std::fstream outfile;
outfile.open("simOut.csv", std::ios::app);
// 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, yawddot, pitchddot, rollddot, I11, I22, I33, "
"I11dot, I22dot, I33dot, Servo1, Servo2, m, thrust, burnElapsed, Fz, "
"LQRx, LQRy"
<< std::endl;
int t = 0;
// Start Sim
@@ -50,11 +31,12 @@ bool sim(struct Vehicle &State, struct Vehicle &PrevState) {
lqrCalc(State);
TVC(State);
vehicleDynamics(State, PrevState, t);
write2CSV(State, outfile, t);
t++;
} while ((State.z > 0) && (State.thrust != 0));
state2vec(State, stateVector, t);
outfile.close();
t++;
} while ((State.z > 0) || (State.thrust > 1));
write2CSV(stateVector);
bool returnValue;
@@ -71,7 +53,7 @@ bool sim(struct Vehicle &State, struct Vehicle &PrevState) {
return returnValue;
}
void burnStartTimeCalc(struct Vehicle &State) {
void burnStartTimeCalc(Vehicle &State) {
double velocity = State.vz;
double h = 0;
@@ -102,7 +84,7 @@ void burnStartTimeCalc(struct Vehicle &State) {
State.simTime = (State.burntime + burnStartTime) * 1000;
}
void thrustSelection(struct Vehicle &State, int t) {
void thrustSelection(Vehicle &State, int t) {
if (State.burnElapsed != 2000) {
// determine where in the thrust curve we're at based on elapsed burn time
@@ -135,7 +117,7 @@ void thrustSelection(struct Vehicle &State, int t) {
State.thrust = -195.78 * State.burnElapsed + 675.11;
}
void lqrCalc(struct Vehicle &State) {
void lqrCalc(Vehicle &State) {
State.I11 = State.mass * ((1 / 12) * pow(State.vehicleHeight, 2) +
pow(State.vehicleRadius, 2) / 4);
@@ -166,7 +148,8 @@ void lqrCalc(struct Vehicle &State) {
// changing gain exponent drastically changes results of LQR
double gain = 0.25 * pow(10, -4);
// Matrix Multiply K with [YPR/2; w123] column vector and divide by moment arm
// Matrix Multiply K with [YPR/2; w123] column vector and divide by moment
// arm
State.LQRx =
gain *
((K12 * State.pitch) / 2 + K15 * State.pitchdot + (K13 * State.roll) / 2 +
@@ -191,7 +174,7 @@ void lqrCalc(struct Vehicle &State) {
State.LQRy = -1 * State.thrust;
}
void TVC(struct Vehicle &State) {
void TVC(Vehicle &State) {
if (State.thrust < 1) {
// Define forces and moments for t = 0
State.Fx = 0;
@@ -309,86 +292,84 @@ void vehicleDynamics(Vehicle &State, Vehicle &PrevState, int t) {
State.roll = integral(State.psidot, PrevState.roll, State.stepSize);
}
PrevState = State;
/*
// Set "prev" values for next timestep
PrevState.I11 = State.I11;
PrevState.I22 = State.I22;
PrevState.I33 = State.I33;
PrevState.yaw = State.yaw;
PrevState.pitch = State.pitch;
PrevState.roll = State.roll;
PrevState.yawdot = State.yawdot;
PrevState.pitchdot = State.pitchdot;
PrevState.rolldot = State.rolldot;
PrevState.yawddot = State.yawddot;
PrevState.pitchddot = State.pitchddot;
PrevState.rollddot = State.rollddot;
PrevState.ax = State.ax;
PrevState.ay = State.ay;
PrevState.az = State.az;
PrevState.vx = State.vx;
PrevState.vy = State.vy;
PrevState.vz = State.vz;
PrevState.x = State.x;
PrevState.y = State.y;
PrevState.z = State.z;
*/
PrevState = State;
}
void write2CSV(struct Vehicle &State, std::fstream &outfile, int t) {
void state2vec(Vehicle &State, outVector &stateVector, int t) {
stateVector.x[t] = State.x;
stateVector.y[t] = State.y;
stateVector.z[t] = State.z;
stateVector.vx[t] = State.vx;
stateVector.vy[t] = State.vy;
stateVector.vz[t] = State.vz;
stateVector.ax[t] = State.ax;
stateVector.ay[t] = State.ay;
stateVector.az[t] = State.az;
stateVector.yaw[t] = State.yaw;
stateVector.pitch[t] = State.pitch;
stateVector.roll[t] = State.roll;
stateVector.yawdot[t] = State.yawdot;
stateVector.pitchdot[t] = State.pitchdot;
stateVector.rolldot[t] = State.rolldot;
stateVector.servo1[t] = State.xServoDegs;
stateVector.servo2[t] = State.yServoDegs;
}
void write2CSV(outVector &stateVector) {
// Deleting any previous output file
if (remove("simOut.csv") != 0)
perror("No file deletion necessary");
else
puts("Previous output file successfully deleted");
// Define and open output file "simOut.csv"
std::fstream outfile;
outfile.open("simOut.csv", std::ios::app);
// 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"
<< std::endl;
std::cout << "Writing to csv...\n";
// writing to output file
outfile << t << ", ";
for (int t = 0; t < stateVector.x.size(); t++) {
outfile << t << ", ";
outfile << State.x << ", ";
outfile << State.y << ", ";
outfile << State.z << ", ";
outfile << stateVector.x[t] << ", ";
outfile << stateVector.y[t] << ", ";
outfile << stateVector.z[t] << ", ";
outfile << State.vx << ", ";
outfile << State.vy << ", ";
outfile << State.vz << ", ";
outfile << stateVector.vx[t] << ", ";
outfile << stateVector.vy[t] << ", ";
outfile << stateVector.vz[t] << ", ";
outfile << State.ax << ", ";
outfile << State.ay << ", ";
outfile << State.az << ", ";
outfile << stateVector.ax[t] << ", ";
outfile << stateVector.ay[t] << ", ";
outfile << stateVector.az[t] << ", ";
outfile << State.yaw * 180 / M_PI << ", ";
outfile << State.pitch * 180 / M_PI << ", ";
outfile << State.roll * 180 / M_PI << ", ";
outfile << stateVector.yaw[t] * 180 / M_PI << ", ";
outfile << stateVector.pitch[t] * 180 / M_PI << ", ";
outfile << stateVector.roll[t] * 180 / M_PI << ", ";
outfile << State.yawdot * 180 / M_PI << ", ";
outfile << State.pitchdot * 180 / M_PI << ", ";
outfile << State.rolldot * 180 / M_PI << ", ";
outfile << stateVector.yawdot[t] * 180 / M_PI << ", ";
outfile << stateVector.pitchdot[t] * 180 / M_PI << ", ";
outfile << stateVector.rolldot[t] * 180 / M_PI << ", ";
outfile << State.yawddot * 180 / M_PI << ", ";
outfile << State.pitchddot * 180 / M_PI << ", ";
outfile << State.rollddot * 180 / M_PI << ", ";
outfile << stateVector.servo1[t] << ", ";
outfile << stateVector.servo2[t] << std::endl;
}
outfile << State.I11 << ", ";
outfile << State.I22 << ", ";
outfile << State.I33 << ", ";
outfile << State.I11dot << ", ";
outfile << State.I22dot << ", ";
outfile << State.I33dot << ", ";
outfile << State.xServoDegs << ", ";
outfile << State.yServoDegs << ", ";
outfile << State.mass << ", ";
outfile << State.thrust << ", ";
outfile << State.burnElapsed << ", ";
outfile << State.Fz << ", ";
outfile << State.LQRx << ", ";
outfile << State.LQRy << std::endl;
outfile.close();
}
double derivative(double x2, double x1, double dt) {