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

Resolve "Separate native and teensy code"

Browse Source
This commit is contained in:
Anson Biggs 2021-11-03 22:33:10 +00:00
parent ad602ca834
commit 66590a33fe
6 changed files with 273 additions and 220 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
.vscode/settings.json vendored
View File

@ -42,7 +42,10 @@
"vector": "cpp", "vector": "cpp",
"cctype": "cpp", "cctype": "cpp",
"sstream": "cpp", "sstream": "cpp",
"string": "cpp" "string": "cpp",
"chrono": "cpp",
"ratio": "cpp",
"thread": "cpp"
}, },
"C_Cpp.clang_format_fallbackStyle": "LLVM", "C_Cpp.clang_format_fallbackStyle": "LLVM",
"editor.formatOnSave": true, "editor.formatOnSave": true,

45
include/Vehicle.h
View File

@ -41,6 +41,51 @@ struct Vehicle {
double simTime; double simTime;
int stepSize; int stepSize;
int time = 0;
}; };
void init_Vehicle(Vehicle &State) {
// PID Gains
State.Kp = -6.8699;
State.Ki = 0;
State.Kd = -0.775;
// Initial Velocity
State.vx = 0; // [m/s]
State.vy = 0; // [m/s]
State.vz = 0; // [m/s]
// Initial YPR
State.yaw = 45 * M_PI / 180; // [rad]
State.pitch = 45 * M_PI / 180; // [rad]
State.roll = 0 * M_PI / 180; // [rad]
// Initial YPRdot
State.yawdot = 1 * M_PI / 180; // [rad/s]
State.pitchdot = -1 * M_PI / 180; // [rad/s]
State.rolldot = 0 * M_PI / 180; // [rad/s]
// Servo Limitation
State.maxServo = 7; // [degs]
State.maxServoRate = 360; // [degs/sec]
// Vehicle Properties
State.massInitial = 1.2; // [kg]
State.vehicleHeight = 0.5318; // [m]
State.vehicleRadius = 0.05105; // [m]
State.momentArm = 0.145; // [m]
// Sim Step Size
State.stepSize = 1; // [ms]
// Other Properties
State.massPropellant = 0.06; // [kg]
State.massBurnout = State.massInitial - State.massPropellant; // [kg]
State.burntime = 3.45 - 0.148; // [s]
State.mdot = State.massPropellant / State.burntime; // [kg/s]
State.mass = State.massInitial; // [kg]
State.burnElapsed = 2000; // [s]
}
#endif #endif

143
include/native.h Normal file
View File

@ -0,0 +1,143 @@
#include "Vehicle.h"
#include <iostream>
void thrustInfo(struct Vehicle &);
void processTVC(struct Vehicle &);
void write2CSV(struct outVector &, struct Vehicle &);
void printSimResults(struct Vehicle &);
void thrustInfo(Vehicle &State) {
if (State.burnElapsed != 2000) {
// determine where in the thrust curve we're at based on elapsed burn time
// as well as current mass
State.burnElapsed = (State.time - State.burnStart) / 1000;
State.mass = State.massInitial - (State.mdot * State.burnElapsed);
}
else if (abs(State.burnVelocity + State.vz) < 0.001) {
// Start burn
State.burnStart = State.time;
State.burnElapsed = 0;
}
else
State.burnElapsed = 2000; // arbitrary number to ensure we don't burn
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) +
60.739 * pow(State.burnElapsed, 4) -
162.99 * pow(State.burnElapsed, 3) +
235.6 * pow(State.burnElapsed, 2) -
174.43 * State.burnElapsed + 67.17;
else if ((State.burnElapsed > 3.382) && (State.burnElapsed < 3.46))
State.thrust = -195.78 * State.burnElapsed - 675.11;
if (State.burnElapsed > 3.45) {
State.thrustFiring = false;
State.thrust = 0;
}
}
void processTVC(Vehicle &State) {
// Vector math to aqcuire thrust vector components
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)) +
(State.mass * g);
// Calculate moment created by Fx and Fy
State.momentX = State.Fx * State.momentArm;
State.momentY = State.Fy * State.momentArm;
State.momentZ = 0;
}
void write2CSV(outVector &stateVector, Vehicle &State) {
// 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,thrustFiring,PIDx,PIDy,thrust"
<< std::endl;
// writing to output file
for (int i = 0; i < State.time; i += State.stepSize) {
outfile << i << ", ";
outfile << stateVector.x[i] << ",";
outfile << stateVector.y[i] << ",";
outfile << stateVector.z[i] << ",";
outfile << stateVector.vx[i] << ",";
outfile << stateVector.vy[i] << ",";
outfile << stateVector.vz[i] << ",";
outfile << stateVector.ax[i] << ",";
outfile << stateVector.ay[i] << ",";
outfile << stateVector.az[i] << ",";
outfile << stateVector.yaw[i] * 180 / M_PI << ",";
outfile << stateVector.pitch[i] * 180 / M_PI << ",";
outfile << stateVector.roll[i] * 180 / M_PI << ",";
outfile << stateVector.yawdot[i] * 180 / M_PI << ",";
outfile << stateVector.pitchdot[i] * 180 / M_PI << ",";
outfile << stateVector.rolldot[i] * 180 / M_PI << ",";
outfile << stateVector.servo1[i] << ",";
outfile << stateVector.servo2[i] << ",";
outfile << stateVector.thrustFiring[i] << ",";
outfile << stateVector.PIDx[i] << ",";
outfile << stateVector.PIDy[i] << ",";
outfile << stateVector.thrust[i] << std::endl;
}
outfile.close();
std::cout << "simOut.csv created successfully.\n" << std::endl;
}
void printSimResults(Vehicle &State) {
double landing_angle =
pow(State.yaw * State.yaw + State.pitch * State.pitch, .5);
double landing_velocity =
pow(State.vx * State.vx + State.vy * State.vy + State.vz * State.vz, .5);
if (landing_angle < 5.0) {
std::cout << "Landing Angle < 5.0 degrees | PASS | ";
} else {
std::cout << "Landing Angle < 5.0 degrees | FAIL | ";
}
std::cout << "Final Angles: [" << State.yaw << ", " << State.pitch << "]"
<< std::endl;
if (landing_velocity < 0.5) {
std::cout << "Landing Velocity < 0.5 m/s | PASS | ";
} else {
std::cout << "Landing Velocity < 0.5 m/s | FAIL | ";
}
std::cout << "Final Velocity: [" << State.vx << ", " << State.vy << ", "
<< State.vz << "]" << std::endl;
std::cout << std::endl << "Simulation Complete\n" << std::endl;
}

180
include/sim.h
View File

@ -4,12 +4,10 @@
#include <iostream> #include <iostream>
void burnStartTimeCalc(struct Vehicle &); void burnStartTimeCalc(struct Vehicle &);
void thrustSelection(struct Vehicle &, int t);
void pidController(struct Vehicle &, struct Vehicle &); void pidController(struct Vehicle &, struct Vehicle &);
void TVC(struct Vehicle &, struct Vehicle &); void TVC(struct Vehicle &, struct Vehicle &);
void vehicleDynamics(struct Vehicle &, struct Vehicle &, int t); void vehicleDynamics(struct Vehicle &, struct Vehicle &);
void state2vec(struct Vehicle &, struct Vehicle &, struct outVector &, int t); void state2vec(struct Vehicle &, struct Vehicle &, struct outVector &);
void write2CSV(struct outVector &, struct Vehicle &, int t);
double derivative(double current, double previous, double step); double derivative(double current, double previous, double step);
double integral(double currentChange, double prevValue, double dt); double integral(double currentChange, double prevValue, double dt);
double limit(double value, double upr, double lwr); double limit(double value, double upr, double lwr);
@ -19,59 +17,6 @@ double limit(double value, double upr, double lwr);
double const dt = 0.01; double const dt = 0.01;
double const g = -9.81; double const g = -9.81;
bool sim(struct Vehicle &State, struct Vehicle &PrevState) {
outVector stateVector;
// Determine when to burn
burnStartTimeCalc(State);
int t = 0;
// Start Sim
do {
vehicleDynamics(State, PrevState, t);
thrustSelection(State, t);
pidController(State, PrevState);
TVC(State, PrevState);
state2vec(State, PrevState, stateVector, t);
//std::cout << State.vz << "\n";
t += State.stepSize;
} while ((State.z > 0.0));
std::cout << t << "\n";
write2CSV(stateVector, State, t);
std::cout << t << "\n";
bool pass = 1;
double landing_angle =
pow(State.yaw * State.yaw + State.pitch * State.pitch, .5);
double landing_velocity =
pow(State.vx * State.vx + State.vy * State.vy + State.vz * State.vz, .5);
if (landing_angle < 5.0) {
std::cout << " Landing Angle < 5° | PASS | ";
} else {
std::cout << " Landing Angle < 5° | FAIL | ";
pass = pass * 0;
}
std::cout << "Final Angles: [" << State.yaw << ", " << State.pitch << "]"
<< std::endl;
if (landing_velocity < 5.0) {
std::cout << "Landing Velocity < 5 m/s | PASS | ";
} else {
std::cout << "Landing Velocity < 5 m/s | FAIL | ";
pass = pass * 0;
}
std::cout << "Final Velocity: [" << State.vx << ", " << State.vy << ", "
<< State.vz << "]" << std::endl;
return pass;
}
void burnStartTimeCalc(Vehicle &State) { void burnStartTimeCalc(Vehicle &State) {
double velocity = State.vz; double velocity = State.vz;
double h = 0; double h = 0;
@ -96,13 +41,15 @@ void burnStartTimeCalc(Vehicle &State) {
h = (((thrust / mass) + g) * dt) + h; h = (((thrust / mass) + g) * dt) + h;
} }
State.z = h + (pow(velocity, 2) / (2 * -g)); // starting height State.z = h + (pow(velocity, 2) / (2 * -g)); // starting height
State.burnVelocity = velocity; // terminal velocity State.z = 18.9;
State.burnVelocity = velocity; // terminal velocity
double burnStartTime = State.burnVelocity / -g; double burnStartTime = State.burnVelocity / -g;
State.simTime = (State.burntime + burnStartTime) * 1000; State.simTime = (State.burntime + burnStartTime) * 1000;
} }
void vehicleDynamics(Vehicle &State, Vehicle &PrevState, int t) { void vehicleDynamics(Vehicle &State, Vehicle &PrevState) {
// Moment of Inertia // Moment of Inertia
State.I11 = State.mass * ((1 / 12) * pow(State.vehicleHeight, 2) + State.I11 = State.mass * ((1 / 12) * pow(State.vehicleHeight, 2) +
pow(State.vehicleRadius, 2) / 4); pow(State.vehicleRadius, 2) / 4);
@ -111,7 +58,7 @@ void vehicleDynamics(Vehicle &State, Vehicle &PrevState, int t) {
State.I33 = State.mass * 0.5 * pow(State.vehicleRadius, 2); State.I33 = State.mass * 0.5 * pow(State.vehicleRadius, 2);
// Idot // Idot
if (t < 0.1) { if (State.time < 0.1) {
State.I11dot = 0; State.I11dot = 0;
State.I22dot = 0; State.I22dot = 0;
State.I33dot = 0; State.I33dot = 0;
@ -180,45 +127,6 @@ void vehicleDynamics(Vehicle &State, Vehicle &PrevState, 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
// as well as current mass
State.burnElapsed = (t - State.burnStart) / 1000;
State.mass = State.massInitial - (State.mdot * State.burnElapsed);
}
else if (abs(State.burnVelocity + State.vz) < 0.001) {
// Start burn
State.burnStart = t;
State.burnElapsed = 0;
}
else
State.burnElapsed = 2000; // arbitrary number to ensure we don't burn
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) +
60.739 * pow(State.burnElapsed, 4) -
162.99 * pow(State.burnElapsed, 3) +
235.6 * pow(State.burnElapsed, 2) -
174.43 * State.burnElapsed + 67.17;
else if ((State.burnElapsed > 3.382) && (State.burnElapsed < 3.46))
State.thrust = -195.78 * State.burnElapsed - 675.11;
if (State.burnElapsed > 3.45) {
State.thrustFiring = false;
State.thrust = 0;
}
}
void pidController(Vehicle &State, struct Vehicle &PrevState) { void pidController(Vehicle &State, struct Vehicle &PrevState) {
// 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) {
@ -290,23 +198,12 @@ void TVC(Vehicle &State, Vehicle &PrevState) {
integral(State.xServoDegsDot, PrevState.xServoDegs, State.stepSize); integral(State.xServoDegsDot, PrevState.xServoDegs, State.stepSize);
State.yServoDegs = State.yServoDegs =
integral(State.yServoDegsDot, PrevState.yServoDegs, State.stepSize); integral(State.yServoDegsDot, PrevState.yServoDegs, State.stepSize);
// Vector math to aqcuire thrust vector components
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)) +
(State.mass * g);
// Calculate moment created by Fx and Fy
State.momentX = State.Fx * State.momentArm;
State.momentY = State.Fy * State.momentArm;
State.momentZ = 0;
} }
} }
void state2vec(Vehicle &State, Vehicle &PrevState, outVector &stateVector, void state2vec(Vehicle &State, Vehicle &PrevState, outVector &stateVector) {
int t) { int t = State.time;
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;
@ -341,63 +238,6 @@ void state2vec(Vehicle &State, Vehicle &PrevState, outVector &stateVector,
PrevState = State; PrevState = State;
} }
void write2CSV(outVector &stateVector, Vehicle &State, int t) {
// 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,thrustFiring,PIDx,PIDy,thrust"
<< std::endl;
// writing to output file
for (int i = 0; i < t; i += State.stepSize) {
outfile << i << ", ";
outfile << stateVector.x[i] << ",";
outfile << stateVector.y[i] << ",";
outfile << stateVector.z[i] << ",";
outfile << stateVector.vx[i] << ",";
outfile << stateVector.vy[i] << ",";
outfile << stateVector.vz[i] << ",";
outfile << stateVector.ax[i] << ",";
outfile << stateVector.ay[i] << ",";
outfile << stateVector.az[i] << ",";
outfile << stateVector.yaw[i] * 180 / M_PI << ",";
outfile << stateVector.pitch[i] * 180 / M_PI << ",";
outfile << stateVector.roll[i] * 180 / M_PI << ",";
outfile << stateVector.yawdot[i] * 180 / M_PI << ",";
outfile << stateVector.pitchdot[i] * 180 / M_PI << ",";
outfile << stateVector.rolldot[i] * 180 / M_PI << ",";
outfile << stateVector.servo1[i] << ",";
outfile << stateVector.servo2[i] << ",";
outfile << stateVector.thrustFiring[i] << ",";
outfile << stateVector.PIDx[i] << ",";
outfile << stateVector.PIDy[i] << ",";
outfile << stateVector.thrust[i] << std::endl;
}
outfile.close();
std::cout << "simOut.csv created successfully.\n" << std::endl;
}
double derivative(double current, double previous, double step) { double derivative(double current, double previous, double step) {
double dxdt = (current - previous) / (step / 1000); double dxdt = (current - previous) / (step / 1000);
return dxdt; return dxdt;

21
include/teensy.h Normal file
View File

@ -0,0 +1,21 @@
#include "Vehicle.h"
void loadCellCalibrate();
void thrustInfo();
void processTVC();
void write2csv();
void loadCellCalibrate() {
// place code to calibrate load cells in here
}
void thrustInfo() {
// place code to retrieve load cell data and convert to a thrust value in here
}
void processTVC() {
// place code to turn angles from TVC function into commands in here
}
void write2csv() {
// place code to make the teensy write to an output file in here
}

99
src/main.cpp
View File

@ -11,58 +11,59 @@
#include "Vehicle.h" #include "Vehicle.h"
#include "sim.h" #include "sim.h"
bool sim(struct Vehicle &); #if defined(_WIN32) || defined(linux)
#include "native.h"
#elif TEENSY
#include "teensy.h"
#endif
Vehicle State;
Vehicle PrevState;
outVector stateVector;
#if defined(_WIN32) || defined(linux)
void setup() {
init_Vehicle(State);
// Determine when to burn
burnStartTimeCalc(State);
}
#elif TEENSY
void setup() {
init_Vehicle(State);
// Determine when to burn
burnStartTimeCalc(State);
loadCellCalibrate();
}
#endif
void loop() {
vehicleDynamics(State, PrevState);
thrustInfo(State);
pidController(State, PrevState);
TVC(State, PrevState);
processTVC(State);
state2vec(State, PrevState, stateVector);
State.time += State.stepSize;
if (State.z < 0.0) {
write2CSV(stateVector, State);
printSimResults(State);
}
}
#if defined(_WIN32) || defined(linux)
int main() { int main() {
Vehicle State;
Vehicle PrevState;
// PID Gains setup();
State.Kp = -6.8699;
State.Ki = 0;
State.Kd = -0.775;
// Initial Velocity do {
State.vx = 0; // [m/s] loop();
State.vy = 0; // [m/s] } while ((State.z > 0.0));
State.vz = 0; // [m/s]
// Initial YPR
State.yaw = 10 * M_PI / 180; // [rad]
State.pitch = 5 * M_PI / 180; // [rad]
State.roll = 0 * M_PI / 180; // [rad]
// Initial YPRdot
State.yawdot = 1 * M_PI / 180; // [rad/s]
State.pitchdot = -1 * M_PI / 180; // [rad/s]
State.rolldot = 0 * M_PI / 180; // [rad/s]
// Servo Limitation
State.maxServo = 7; // [degs]
State.maxServoRate = 360; // [degs/sec]
// Vehicle Properties
State.massInitial = 1.2; // [kg]
State.vehicleHeight = 0.5318; // [m]
State.vehicleRadius = 0.05105; // [m]
State.momentArm = 0.145; // [m]
// Sim Step Size
State.stepSize = 1; // [ms]
// Other Properties
State.massPropellant = 0.06; // [kg]
State.massBurnout = State.massInitial - State.massPropellant; // [kg]
State.burntime = 3.45 - 0.148; // [s]
State.mdot = State.massPropellant / State.burntime; // [kg/s]
State.mass = State.massInitial; // [kg]
State.burnElapsed = 2000; // [s]
PrevState.thrust = 0; // [N]
bool outcome = sim(State, PrevState);
std::cout << std::endl << "Simulation Complete 🚀" << std::endl;
// ^^^
// 50% chance this makes Mattys linux crash
return 0; return 0;
} }
#endif