mirror of
https://gitlab.com/lander-team/lander-cpp.git
synced 2025-06-17 15:46:39 +00:00
Compare commits
No commits in common. "main" and "LQR" have entirely different histories.
6
.gitignore
vendored
6
.gitignore
vendored
@ -2,9 +2,3 @@
|
|||||||
*.csv
|
*.csv
|
||||||
output
|
output
|
||||||
public
|
public
|
||||||
|
|
||||||
.pio
|
|
||||||
.vscode/.browse.c_cpp.db*
|
|
||||||
.vscode/c_cpp_properties.json
|
|
||||||
.vscode/launch.json
|
|
||||||
.vscode/ipch
|
|
||||||
|
|||||||
@ -13,14 +13,14 @@ build:
|
|||||||
stage: build
|
stage: build
|
||||||
script:
|
script:
|
||||||
- mkdir public
|
- mkdir public
|
||||||
- g++ ./src/main.cpp -I include -o ./public/sim.out -D NATIVE -Wall
|
- g++ ./src/main.cpp -I include -o ./public/sim.out
|
||||||
- ./public/sim.out
|
- ./public/sim.out
|
||||||
- mv simOut.csv public
|
- mv simOut.csv public
|
||||||
artifacts:
|
artifacts:
|
||||||
paths:
|
paths:
|
||||||
- public
|
- public
|
||||||
|
|
||||||
pythonplots:
|
plots:
|
||||||
# stage: build
|
# stage: build
|
||||||
image: python:3.9-buster
|
image: python:3.9-buster
|
||||||
dependencies:
|
dependencies:
|
||||||
@ -32,27 +32,11 @@ pythonplots:
|
|||||||
paths:
|
paths:
|
||||||
- public
|
- public
|
||||||
|
|
||||||
juliaplots:
|
|
||||||
image: julia:1.7
|
|
||||||
dependencies:
|
|
||||||
- build
|
|
||||||
script:
|
|
||||||
- apt-get update
|
|
||||||
- apt-get install -y unzip
|
|
||||||
- julia -e 'using Pkg; Pkg.activate("./juliaHelpers/"); Pkg.instantiate()'
|
|
||||||
- julia --project=./juliaHelpers ./juliaHelpers/simplot.jl
|
|
||||||
artifacts:
|
|
||||||
paths:
|
|
||||||
- public
|
|
||||||
only:
|
|
||||||
- main
|
|
||||||
|
|
||||||
pages:
|
pages:
|
||||||
stage: deploy
|
stage: deploy
|
||||||
dependencies:
|
dependencies:
|
||||||
- build
|
- build
|
||||||
- pythonplots
|
- plots
|
||||||
- juliaplots
|
|
||||||
script:
|
script:
|
||||||
- ""
|
- ""
|
||||||
artifacts:
|
artifacts:
|
||||||
|
|||||||
22
.vscode/c_cpp_properties.json
vendored
Normal file
22
.vscode/c_cpp_properties.json
vendored
Normal file
@ -0,0 +1,22 @@
|
|||||||
|
{
|
||||||
|
"configurations": [
|
||||||
|
{
|
||||||
|
"name": "Win32",
|
||||||
|
"includePath": [
|
||||||
|
"${workspaceFolder}/**",
|
||||||
|
"${workspaceFolder}/include"
|
||||||
|
],
|
||||||
|
"defines": [
|
||||||
|
"_DEBUG",
|
||||||
|
"UNICODE",
|
||||||
|
"_UNICODE"
|
||||||
|
],
|
||||||
|
"windowsSdkVersion": "10.0.18362.0",
|
||||||
|
"compilerPath": "C:\\Program Files (x86)\\Microsoft Visual Studio\\2019\\Community\\VC\\Tools\\MSVC\\14.25.28610\\bin\\Hostx64\\x64\\cl.exe",
|
||||||
|
"cStandard": "c17",
|
||||||
|
"cppStandard": "c++17",
|
||||||
|
"intelliSenseMode": "windows-msvc-x64"
|
||||||
|
}
|
||||||
|
],
|
||||||
|
"version": 4
|
||||||
|
}
|
||||||
4
.vscode/extensions.json
vendored
4
.vscode/extensions.json
vendored
@ -1,10 +1,6 @@
|
|||||||
{
|
{
|
||||||
// See http://go.microsoft.com/fwlink/?LinkId=827846
|
|
||||||
// for the documentation about the extensions.json format
|
|
||||||
"recommendations": [
|
"recommendations": [
|
||||||
"ms-vscode.cpptools",
|
"ms-vscode.cpptools",
|
||||||
"platformio.platformio-ide",
|
|
||||||
"usernamehw.errorlens",
|
|
||||||
"wayou.vscode-todo-highlight"
|
"wayou.vscode-todo-highlight"
|
||||||
]
|
]
|
||||||
}
|
}
|
||||||
29
.vscode/launch.json
vendored
Normal file
29
.vscode/launch.json
vendored
Normal file
@ -0,0 +1,29 @@
|
|||||||
|
{
|
||||||
|
// Use IntelliSense to learn about possible attributes.
|
||||||
|
// Hover to view descriptions of existing attributes.
|
||||||
|
// For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387
|
||||||
|
"version": "0.2.0",
|
||||||
|
"configurations": [
|
||||||
|
{
|
||||||
|
"name": "g++ - Build and debug simulation",
|
||||||
|
"type": "cppdbg",
|
||||||
|
"request": "launch",
|
||||||
|
"program": "${workspaceFolder}/main.exe",
|
||||||
|
"args": [],
|
||||||
|
"stopAtEntry": false,
|
||||||
|
"cwd": "${fileDirname}",
|
||||||
|
"environment": [],
|
||||||
|
"externalConsole": false,
|
||||||
|
"MIMode": "gdb",
|
||||||
|
"miDebuggerPath": "gdb.exe",
|
||||||
|
"setupCommands": [
|
||||||
|
{
|
||||||
|
"description": "Enable pretty-printing for gdb",
|
||||||
|
"text": "-enable-pretty-printing",
|
||||||
|
"ignoreFailures": true
|
||||||
|
}
|
||||||
|
],
|
||||||
|
"preLaunchTask": "buildSim"
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
9
.vscode/settings.json
vendored
9
.vscode/settings.json
vendored
@ -39,14 +39,7 @@
|
|||||||
"xstring": "cpp",
|
"xstring": "cpp",
|
||||||
"xtr1common": "cpp",
|
"xtr1common": "cpp",
|
||||||
"xutility": "cpp",
|
"xutility": "cpp",
|
||||||
"vector": "cpp",
|
"vector": "cpp"
|
||||||
"cctype": "cpp",
|
|
||||||
"sstream": "cpp",
|
|
||||||
"string": "cpp",
|
|
||||||
"chrono": "cpp",
|
|
||||||
"ratio": "cpp",
|
|
||||||
"thread": "cpp",
|
|
||||||
"string_view": "cpp"
|
|
||||||
},
|
},
|
||||||
"C_Cpp.clang_format_fallbackStyle": "LLVM",
|
"C_Cpp.clang_format_fallbackStyle": "LLVM",
|
||||||
"editor.formatOnSave": true,
|
"editor.formatOnSave": true,
|
||||||
|
|||||||
5
.vscode/tasks.json
vendored
5
.vscode/tasks.json
vendored
@ -2,7 +2,7 @@
|
|||||||
"version": "2.0.0",
|
"version": "2.0.0",
|
||||||
"tasks": [
|
"tasks": [
|
||||||
{
|
{
|
||||||
"type": "process",
|
"type": "cppbuild",
|
||||||
"label": "buildSim",
|
"label": "buildSim",
|
||||||
"command": "g++",
|
"command": "g++",
|
||||||
"args": [
|
"args": [
|
||||||
@ -11,8 +11,7 @@
|
|||||||
"-o",
|
"-o",
|
||||||
"${workspaceFolder}/main.exe",
|
"${workspaceFolder}/main.exe",
|
||||||
"-I",
|
"-I",
|
||||||
"${workspaceFolder}/include",
|
"${workspaceFolder}/include"
|
||||||
"-Wall"
|
|
||||||
],
|
],
|
||||||
"options": {
|
"options": {
|
||||||
"cwd": "${fileDirname}"
|
"cwd": "${fileDirname}"
|
||||||
|
|||||||
10
README.md
10
README.md
@ -29,12 +29,8 @@ Download the raw CSV: https://lander-team.gitlab.io/lander-cpp/simOut.csv
|
|||||||
|
|
||||||
Download Linux Binaries: https://lander-team.gitlab.io/lander-cpp/sim.out
|
Download Linux Binaries: https://lander-team.gitlab.io/lander-cpp/sim.out
|
||||||
|
|
||||||
### Plots
|
|
||||||
|
|
||||||
Click on the images for interactive versions.
|
|
||||||
|
|
||||||
<a href="https://lander-team.gitlab.io/lander-cpp/plot1.html" rel="Accel-Vel-ALt"></a>
|
|
||||||
|
|
||||||
<a href="https://lander-team.gitlab.io/lander-cpp/plot2.html" rel="Euler Angles vs Time"></a>
|
|
||||||
|
|
||||||
<a href="https://lander-team.gitlab.io/lander-cpp/plot3.html" rel="Servo Position vs Time"></a>
|
|
||||||
|
|||||||
@ -20,76 +20,20 @@ struct Vehicle {
|
|||||||
double burntime;
|
double burntime;
|
||||||
double burnVelocity;
|
double burnVelocity;
|
||||||
double thrust, burnElapsed, burnStart;
|
double thrust, burnElapsed, burnStart;
|
||||||
int thrustFiring = 0; // 0: Pre-burn, 1: Mid-burn, 2: Post-burn
|
bool thrustFiring = false;
|
||||||
|
;
|
||||||
|
|
||||||
double PIDx, PIDy, Fx, Fy, Fz, F;
|
double LQRx, LQRy, Fx, Fy, Fz;
|
||||||
double momentX, momentY, momentZ;
|
double momentX, momentY, momentZ;
|
||||||
|
|
||||||
double I11, I22, I33;
|
double I11, I22, I33;
|
||||||
double I11dot, I22dot, I33dot;
|
double I11dot, I22dot, I33dot;
|
||||||
|
|
||||||
double maxServo;
|
int maxServo;
|
||||||
double maxServoRate;
|
|
||||||
double xServoDegs, yServoDegs;
|
double xServoDegs, yServoDegs;
|
||||||
double xServoDegsDot, yServoDegsDot;
|
|
||||||
|
|
||||||
double Kp, Ki, Kd;
|
|
||||||
double yError, yPrevError;
|
|
||||||
double pError, pPrevError;
|
|
||||||
double i_yError, i_pError = 0.0;
|
|
||||||
double d_yError, d_pError;
|
|
||||||
|
|
||||||
double simTime;
|
double simTime;
|
||||||
double stepSize;
|
int stepSize;
|
||||||
double stepDuration;
|
|
||||||
|
|
||||||
double time = 0.0;
|
|
||||||
|
|
||||||
double lc0, lc1, lc2, lc3 = 0.0;
|
|
||||||
double lc0_processed, lc1_processed, lc2_processed, lc3_processed = 0.0;
|
|
||||||
};
|
};
|
||||||
|
|
||||||
void init_Vehicle(Vehicle &State) {
|
|
||||||
// PID Gains
|
|
||||||
State.Kp = -6.8699;
|
|
||||||
State.Ki = 0.0;
|
|
||||||
State.Kd = -0.775;
|
|
||||||
|
|
||||||
// Initial Velocity
|
|
||||||
State.vx = 0.0; // [m/s]
|
|
||||||
State.vy = 0.0; // [m/s]
|
|
||||||
State.vz = 0.0; // [m/s]
|
|
||||||
|
|
||||||
// Initial YPR
|
|
||||||
State.yaw = 45.0 * M_PI / 180.0; // [rad]
|
|
||||||
State.pitch = 45.0 * M_PI / 180.0; // [rad]
|
|
||||||
State.roll = 0.0 * M_PI / 180.0; // [rad]
|
|
||||||
|
|
||||||
// Initial YPRdot
|
|
||||||
State.yawdot = 1.0 * M_PI / 180.0; // [rad/s]
|
|
||||||
State.pitchdot = -1.0 * M_PI / 180.0; // [rad/s]
|
|
||||||
State.rolldot = 0.0 * M_PI / 180.0; // [rad/s]
|
|
||||||
|
|
||||||
// Servo Limitation
|
|
||||||
State.maxServo = 7.0; // [degs]
|
|
||||||
State.maxServoRate = 360.0; // [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 = 5.0; // [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
|
||||||
129
include/native.h
129
include/native.h
@ -1,129 +0,0 @@
|
|||||||
#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 ((std::abs(State.burnVelocity + State.vz) < 1.03) &&
|
|
||||||
(State.thrustFiring == 0)) {
|
|
||||||
// Start burn
|
|
||||||
State.burnStart = State.time;
|
|
||||||
State.burnElapsed = 0.0;
|
|
||||||
State.thrustFiring = 1;
|
|
||||||
|
|
||||||
getThrust(State);
|
|
||||||
|
|
||||||
} else if (State.thrustFiring == 1) {
|
|
||||||
State.burnElapsed = (State.time - State.burnStart) / 1000.0;
|
|
||||||
State.mass = State.massInitial - (State.mdot * State.burnElapsed);
|
|
||||||
|
|
||||||
getThrust(State);
|
|
||||||
|
|
||||||
} else {
|
|
||||||
State.thrust = 0.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) {
|
|
||||||
State.yaw = State.yaw * 180 / M_PI;
|
|
||||||
State.pitch = State.pitch * 180 / M_PI;
|
|
||||||
State.roll = State.roll * 180 / M_PI;
|
|
||||||
|
|
||||||
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 < 1.0) {
|
|
||||||
std::cout << "Landing Velocity < 1.0 m/s | PASS | ";
|
|
||||||
} else {
|
|
||||||
std::cout << "Landing Velocity < 1.0 m/s | FAIL | ";
|
|
||||||
}
|
|
||||||
std::cout << "Final Velocity: [" << State.vx << ", " << State.vy << ", "
|
|
||||||
<< State.vz << "]" << std::endl;
|
|
||||||
|
|
||||||
std::cout << std::endl << "Simulation Complete\n" << std::endl;
|
|
||||||
}
|
|
||||||
@ -30,11 +30,6 @@ struct outVector {
|
|||||||
std::vector<double> servo2 = std::vector<double>(length, 0.0);
|
std::vector<double> servo2 = std::vector<double>(length, 0.0);
|
||||||
|
|
||||||
std::vector<bool> thrustFiring = std::vector<bool>(length, 0.0);
|
std::vector<bool> thrustFiring = std::vector<bool>(length, 0.0);
|
||||||
|
|
||||||
std::vector<double> PIDx = std::vector<double>(length, 0.0);
|
|
||||||
std::vector<double> PIDy = std::vector<double>(length, 0.0);
|
|
||||||
|
|
||||||
std::vector<double> thrust = std::vector<double>(length, 0.0);
|
|
||||||
};
|
};
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
465
include/sim.h
465
include/sim.h
@ -1,28 +1,63 @@
|
|||||||
#include "Vehicle.h"
|
#include "Vehicle.h"
|
||||||
#include "outVector.h"
|
#include "outVector.h"
|
||||||
|
|
||||||
#include <iostream>
|
|
||||||
|
|
||||||
void burnStartTimeCalc(struct Vehicle &);
|
void burnStartTimeCalc(struct Vehicle &);
|
||||||
void pidController(struct Vehicle &, struct Vehicle &);
|
void thrustSelection(struct Vehicle &, int t);
|
||||||
void TVC(struct Vehicle &, struct Vehicle &);
|
void lqrCalc(struct Vehicle &);
|
||||||
void vehicleDynamics(struct Vehicle &, struct Vehicle &);
|
void TVC(struct Vehicle &);
|
||||||
void state2vec(struct Vehicle &, struct Vehicle &, struct outVector &);
|
void vehicleDynamics(struct Vehicle &, struct Vehicle &, int t);
|
||||||
double derivative(double current, double previous, double step);
|
void state2vec(struct Vehicle &, struct outVector &, int t);
|
||||||
double integral(double currentChange, double prevValue, double dt);
|
void write2CSV(struct outVector &, struct Vehicle &);
|
||||||
double limit(double value, double upr, double lwr);
|
double derivative(double x2, double x1, double dt);
|
||||||
void getThrust(struct Vehicle &);
|
double integral(double x2, double x1, double dt);
|
||||||
|
|
||||||
// Any parameters that are constants should be declared here instead of
|
// Any parameters that are constants should be declared here instead of buried
|
||||||
// buried in code
|
// in code
|
||||||
double const dt = 0.01;
|
double const dt = 0.001;
|
||||||
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 {
|
||||||
|
thrustSelection(State, t);
|
||||||
|
lqrCalc(State);
|
||||||
|
TVC(State);
|
||||||
|
vehicleDynamics(State, PrevState, t);
|
||||||
|
state2vec(State, stateVector, t);
|
||||||
|
|
||||||
|
t++;
|
||||||
|
} while ((State.z > 0.0) || (State.thrust > 1.0));
|
||||||
|
|
||||||
|
write2CSV(stateVector, State);
|
||||||
|
|
||||||
|
bool returnValue;
|
||||||
|
|
||||||
|
if (abs(State.vz) < 5) {
|
||||||
|
if ((abs(State.yaw) < 5) && (abs(State.pitch) < 5)) {
|
||||||
|
returnValue = 1;
|
||||||
|
} else {
|
||||||
|
returnValue = 0;
|
||||||
|
}
|
||||||
|
} else {
|
||||||
|
returnValue = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
return returnValue;
|
||||||
|
}
|
||||||
|
|
||||||
void burnStartTimeCalc(Vehicle &State) {
|
void burnStartTimeCalc(Vehicle &State) {
|
||||||
double velocity = State.vz;
|
double velocity = State.vz;
|
||||||
double h = 0.0;
|
double h = 0;
|
||||||
|
|
||||||
double mass, thrust;
|
double a, j, mass, thrust;
|
||||||
|
|
||||||
// Piecewise functions for F15 thrust curve
|
// Piecewise functions for F15 thrust curve
|
||||||
for (double i = 0.148; i < 3.450; i = i + dt) {
|
for (double i = 0.148; i < 3.450; i = i + dt) {
|
||||||
@ -37,88 +72,205 @@ void burnStartTimeCalc(Vehicle &State) {
|
|||||||
|
|
||||||
else if ((i > 3.382) && (i < 3.46))
|
else if ((i > 3.382) && (i < 3.46))
|
||||||
thrust = -195.78 * i + 675.11;
|
thrust = -195.78 * i + 675.11;
|
||||||
else
|
|
||||||
thrust = 0.0;
|
|
||||||
|
|
||||||
velocity = (((thrust / mass) + g) * dt) + velocity;
|
velocity = (((thrust / mass) + g) * dt) + velocity;
|
||||||
h = (((thrust / mass) + g) * dt) + h;
|
h = velocity * dt + h;
|
||||||
}
|
}
|
||||||
State.z = h + (pow(velocity, 2) / (2.0 * -g)); // starting height
|
|
||||||
State.z = 19.05;
|
State.z = h + (pow(velocity, 2) / (2 * -g)); // starting height
|
||||||
State.burnVelocity = velocity; // terminal velocity
|
State.burnVelocity = velocity; // terminal velocity
|
||||||
|
|
||||||
double burnStartTime = State.burnVelocity / -g;
|
double burnStartTime = State.burnVelocity / -g;
|
||||||
State.simTime = (State.burntime + burnStartTime) * 1000.0;
|
State.simTime = (State.burntime + burnStartTime) * 1000;
|
||||||
}
|
}
|
||||||
|
|
||||||
void vehicleDynamics(Vehicle &State, Vehicle &PrevState) {
|
void thrustSelection(Vehicle &State, int t) {
|
||||||
|
|
||||||
// Moment of Inertia
|
if (State.burnElapsed != 2000) {
|
||||||
State.I11 = State.mass * ((1 / 12.0) * pow(State.vehicleHeight, 2) +
|
// determine where in the thrust curve we're at based on elapsed burn time
|
||||||
pow(State.vehicleRadius, 2) / 4.0);
|
// as well as current mass
|
||||||
State.I22 = State.mass * ((1 / 12.0) * pow(State.vehicleHeight, 2) +
|
State.burnElapsed = (t - State.burnStart) / 1000;
|
||||||
pow(State.vehicleRadius, 2) / 4.0);
|
State.mass = State.massInitial - (State.mdot * State.burnElapsed);
|
||||||
|
}
|
||||||
|
|
||||||
|
else if (abs(State.burnVelocity + State.vz) < .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;
|
||||||
|
}
|
||||||
|
|
||||||
|
void lqrCalc(Vehicle &State) {
|
||||||
|
|
||||||
|
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);
|
State.I33 = State.mass * 0.5 * pow(State.vehicleRadius, 2);
|
||||||
|
|
||||||
|
// Paste in Values from gainCalc.m
|
||||||
|
double K11 = 39.54316;
|
||||||
|
double K12 = 0.00000;
|
||||||
|
double K13 = -0.00000;
|
||||||
|
double K14 = 39.55769;
|
||||||
|
double K15 = 0.00000;
|
||||||
|
double K16 = 0.00000;
|
||||||
|
double K21 = 0.00000;
|
||||||
|
double K22 = 39.54316;
|
||||||
|
double K23 = 0.00000;
|
||||||
|
double K24 = 0.00000;
|
||||||
|
double K25 = 39.55769;
|
||||||
|
double K26 = 0.00000;
|
||||||
|
double K31 = 0.00000;
|
||||||
|
double K32 = 0.00000;
|
||||||
|
double K33 = 39.54316;
|
||||||
|
double K34 = 0.00000;
|
||||||
|
double K35 = 0.00000;
|
||||||
|
double K36 = 39.54394;
|
||||||
|
|
||||||
|
// 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
|
||||||
|
State.LQRx =
|
||||||
|
gain *
|
||||||
|
((K12 * State.pitch) / 2 + K15 * State.pitchdot + (K13 * State.roll) / 2 +
|
||||||
|
K16 * State.rolldot + (K11 * State.yaw) / 2 + K14 * State.yawdot) /
|
||||||
|
-State.momentArm;
|
||||||
|
State.LQRy =
|
||||||
|
gain *
|
||||||
|
((K22 * State.pitch) / 2 + K25 * State.pitchdot + (K23 * State.roll) / 2 +
|
||||||
|
K26 * State.rolldot + (K21 * State.yaw) / 2 + K24 * State.yawdot) /
|
||||||
|
-State.momentArm;
|
||||||
|
|
||||||
|
// LQR Force limiter X
|
||||||
|
if (State.LQRx > State.thrust)
|
||||||
|
State.LQRx = State.thrust;
|
||||||
|
else if (State.LQRx < -1 * State.thrust)
|
||||||
|
State.LQRx = -1 * State.thrust;
|
||||||
|
|
||||||
|
// LQR Force limiter Y
|
||||||
|
if (State.LQRy > State.thrust)
|
||||||
|
State.LQRy = State.thrust;
|
||||||
|
else if (State.LQRy < -1 * State.thrust)
|
||||||
|
State.LQRy = -1 * State.thrust;
|
||||||
|
}
|
||||||
|
|
||||||
|
void TVC(Vehicle &State) {
|
||||||
|
if (State.thrust < 1) {
|
||||||
|
// Define forces and moments for t = 0
|
||||||
|
State.Fx = 0;
|
||||||
|
State.Fy = 0;
|
||||||
|
State.Fz = g * State.massInitial;
|
||||||
|
|
||||||
|
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);
|
||||||
|
|
||||||
|
// Servo position limiter
|
||||||
|
if (State.xServoDegs > State.maxServo)
|
||||||
|
State.xServoDegs = State.maxServo;
|
||||||
|
else if (State.xServoDegs < -1 * State.maxServo)
|
||||||
|
State.xServoDegs = -1 * State.maxServo;
|
||||||
|
|
||||||
|
// Convert servo position to degrees for comparison to max allowable
|
||||||
|
State.yServoDegs = (180 / M_PI) * asin(State.LQRy / State.thrust);
|
||||||
|
|
||||||
|
// Servo position limiter
|
||||||
|
if (State.yServoDegs > State.maxServo)
|
||||||
|
State.yServoDegs = State.maxServo;
|
||||||
|
else if (State.yServoDegs < -1 * State.maxServo)
|
||||||
|
State.yServoDegs = -1 * State.maxServo;
|
||||||
|
|
||||||
|
// 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 vehicleDynamics(Vehicle &State, Vehicle &PrevState, int t) {
|
||||||
// Idot
|
// Idot
|
||||||
if (State.time < 0.1) {
|
if (t < 1) {
|
||||||
State.I11dot = 0.0;
|
State.I11dot = 0;
|
||||||
State.I22dot = 0.0;
|
State.I22dot = 0;
|
||||||
State.I33dot = 0.0;
|
State.I33dot = 0;
|
||||||
|
|
||||||
State.x = 0.0;
|
|
||||||
State.y = 0.0;
|
|
||||||
|
|
||||||
State.ax = 0.0;
|
|
||||||
State.ay = 0.0;
|
|
||||||
State.az = State.Fz / State.massInitial;
|
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
State.I11dot = derivative(State.I11, PrevState.I11, State.stepSize);
|
State.I11dot = derivative(State.I11, PrevState.I11, State.stepSize);
|
||||||
State.I22dot = derivative(State.I22, PrevState.I22, State.stepSize);
|
State.I22dot = derivative(State.I22, PrevState.I22, State.stepSize);
|
||||||
State.I33dot = derivative(State.I33, PrevState.I33, State.stepSize);
|
State.I33dot = derivative(State.I33, PrevState.I33, State.stepSize);
|
||||||
|
}
|
||||||
|
|
||||||
// pdot, qdot, rdot
|
// pdot, qdot, rdot
|
||||||
State.yawddot = (State.momentX - State.I11dot * PrevState.yawdot +
|
State.yawddot = (State.momentX - State.I11dot * State.yawdot +
|
||||||
State.I22 * PrevState.pitchdot * PrevState.rolldot -
|
State.I22 * State.pitchdot * State.rolldot -
|
||||||
State.I33 * PrevState.pitchdot * PrevState.rolldot) /
|
State.I33 * State.pitchdot * State.rolldot) /
|
||||||
State.I11;
|
State.I11;
|
||||||
State.pitchddot = (State.momentY - State.I22dot * PrevState.pitchdot -
|
State.pitchddot = (State.momentY - State.I22dot * State.pitchdot -
|
||||||
State.I11 * PrevState.rolldot * PrevState.yawdot +
|
State.I11 * State.rolldot * State.yawdot +
|
||||||
State.I33 * PrevState.rolldot * PrevState.yawdot) /
|
State.I33 * State.rolldot * State.yawdot) /
|
||||||
State.I22;
|
State.I22;
|
||||||
State.rollddot = (State.momentZ - State.I33dot * PrevState.rolldot +
|
State.rollddot = (State.momentZ - State.I33dot * State.rolldot +
|
||||||
State.I11 * PrevState.pitchdot * PrevState.yawdot -
|
State.I11 * State.pitchdot * State.yawdot -
|
||||||
State.I22 * PrevState.pitchdot * PrevState.yawdot) /
|
State.I22 * State.pitchdot * State.yawdot) /
|
||||||
State.I33;
|
State.I33;
|
||||||
|
|
||||||
|
if (t < 1) {
|
||||||
|
State.x = 0;
|
||||||
|
State.y = 0;
|
||||||
|
|
||||||
|
State.ax = 0;
|
||||||
|
State.ay = 0;
|
||||||
|
State.az = State.Fz / State.massInitial;
|
||||||
|
}
|
||||||
|
|
||||||
|
else {
|
||||||
// p, q, r
|
// p, q, r
|
||||||
State.yawdot = integral(State.yawddot, PrevState.yawdot, State.stepSize);
|
State.yawdot = integral(State.yawddot, PrevState.yawdot, State.stepSize);
|
||||||
State.pitchdot =
|
State.pitchdot =
|
||||||
integral(State.pitchddot, PrevState.pitchdot, State.stepSize);
|
integral(State.pitchddot, PrevState.pitchdot, State.stepSize);
|
||||||
State.rolldot = integral(State.rollddot, PrevState.rolldot, 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
|
// ax ay az
|
||||||
State.ax = (State.Fx / State.mass);
|
State.ax = (State.Fx / State.mass) +
|
||||||
State.ay = (State.Fy / State.mass);
|
(State.pitchdot * State.vz - State.rolldot * State.vy);
|
||||||
State.az = (State.Fz / State.mass);
|
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
|
// vx vy vz in Body frame
|
||||||
State.vx = integral(State.ax, PrevState.vx, State.stepSize);
|
State.vx = integral(State.ax, PrevState.vx, State.stepSize);
|
||||||
State.vy = integral(State.ay, PrevState.vy, State.stepSize);
|
State.vy = integral(State.ay, PrevState.vy, State.stepSize);
|
||||||
State.vz = integral(State.az, PrevState.vz, State.stepSize);
|
State.vz = integral(State.az, PrevState.vz, State.stepSize);
|
||||||
@ -127,85 +279,27 @@ void vehicleDynamics(Vehicle &State, Vehicle &PrevState) {
|
|||||||
State.x = integral(State.vx, PrevState.x, State.stepSize);
|
State.x = integral(State.vx, PrevState.x, State.stepSize);
|
||||||
State.y = integral(State.vy, PrevState.y, State.stepSize);
|
State.y = integral(State.vy, PrevState.y, State.stepSize);
|
||||||
State.z = integral(State.vz, PrevState.z, State.stepSize);
|
State.z = integral(State.vz, PrevState.z, State.stepSize);
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void pidController(Vehicle &State, struct Vehicle &PrevState) {
|
// Euler Angles
|
||||||
// Make sure we start reacting when we start burning
|
State.phidot = State.yawdot + (State.pitchdot * sin(State.yaw) +
|
||||||
if (State.thrust > 0.01) {
|
State.rolldot * cos(State.yaw)) *
|
||||||
|
(sin(State.pitch) / cos(State.pitch));
|
||||||
|
State.thetadot =
|
||||||
|
State.pitchdot * cos(State.yaw) - State.rolldot * sin(State.pitch);
|
||||||
|
State.psidot =
|
||||||
|
(State.pitchdot * sin(State.yaw) + State.rolldot * cos(State.yaw)) /
|
||||||
|
cos(State.pitch);
|
||||||
|
|
||||||
State.yError = State.yaw;
|
State.yaw = integral(State.phidot, PrevState.yaw, State.stepSize);
|
||||||
State.pError = State.pitch;
|
State.pitch = integral(State.thetadot, PrevState.pitch, State.stepSize);
|
||||||
|
State.roll = integral(State.psidot, PrevState.roll, State.stepSize);
|
||||||
// Integral of Error
|
|
||||||
State.i_yError = integral(State.yError, State.i_yError, State.stepSize);
|
|
||||||
State.i_pError = integral(State.pError, State.i_pError, State.stepSize);
|
|
||||||
|
|
||||||
// Derivative of Error
|
|
||||||
State.d_yError = derivative(State.yError, PrevState.yError, State.stepSize);
|
|
||||||
State.d_pError = derivative(State.pError, PrevState.pError, State.stepSize);
|
|
||||||
|
|
||||||
// TVC block properly
|
|
||||||
State.PIDx = (State.Kp * State.yError + State.Ki * State.i_yError +
|
|
||||||
State.Kd * State.d_yError) /
|
|
||||||
State.momentArm;
|
|
||||||
State.PIDy = (State.Kp * State.pError + State.Ki * State.i_pError +
|
|
||||||
State.Kd * State.d_pError) /
|
|
||||||
State.momentArm;
|
|
||||||
|
|
||||||
} else {
|
|
||||||
State.PIDx = 0.0;
|
|
||||||
State.PIDy = 0.0;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// PID Force Limiter
|
// Set "prev" values for next timestep
|
||||||
State.PIDx = limit(State.PIDx, State.thrust, -State.thrust);
|
PrevState = State;
|
||||||
State.PIDy = limit(State.PIDy, State.thrust, -State.thrust);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void TVC(Vehicle &State, Vehicle &PrevState) {
|
void state2vec(Vehicle &State, outVector &stateVector, int t) {
|
||||||
if (State.thrust < 0.1) {
|
|
||||||
// Define forces and moments for t = 0
|
|
||||||
State.Fx = 0.0;
|
|
||||||
State.Fy = 0.0;
|
|
||||||
State.Fz = g * State.massInitial;
|
|
||||||
|
|
||||||
State.momentX = 0.0;
|
|
||||||
State.momentY = 0.0;
|
|
||||||
State.momentZ = 0.0;
|
|
||||||
|
|
||||||
} else {
|
|
||||||
// Convert servo position to degrees for comparison to max allowable
|
|
||||||
State.xServoDegs = (180.0 / M_PI) * asin(State.PIDx / State.thrust);
|
|
||||||
State.yServoDegs = (180.0 / M_PI) * asin(State.PIDy / State.thrust);
|
|
||||||
|
|
||||||
// Limit Servo Position
|
|
||||||
State.xServoDegs = limit(State.xServoDegs, State.maxServo, -State.maxServo);
|
|
||||||
State.yServoDegs = limit(State.yServoDegs, State.maxServo, -State.maxServo);
|
|
||||||
|
|
||||||
// Servo Rate
|
|
||||||
State.xServoDegsDot =
|
|
||||||
derivative(State.xServoDegs, PrevState.xServoDegs, State.stepSize);
|
|
||||||
State.yServoDegsDot =
|
|
||||||
derivative(State.yServoDegs, PrevState.yServoDegs, State.stepSize);
|
|
||||||
|
|
||||||
// Limit Servo Rate
|
|
||||||
State.xServoDegsDot =
|
|
||||||
limit(State.xServoDegsDot, State.maxServoRate, -State.maxServoRate);
|
|
||||||
State.yServoDegsDot =
|
|
||||||
limit(State.yServoDegsDot, State.maxServoRate, -State.maxServoRate);
|
|
||||||
|
|
||||||
// Back to Degrees
|
|
||||||
State.xServoDegs =
|
|
||||||
integral(State.xServoDegsDot, PrevState.xServoDegs, State.stepSize);
|
|
||||||
State.yServoDegs =
|
|
||||||
integral(State.yServoDegsDot, PrevState.yServoDegs, State.stepSize);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void state2vec(Vehicle &State, Vehicle &PrevState, outVector &stateVector) {
|
|
||||||
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;
|
||||||
@ -230,51 +324,66 @@ void state2vec(Vehicle &State, Vehicle &PrevState, outVector &stateVector) {
|
|||||||
stateVector.servo2[t] = State.yServoDegs;
|
stateVector.servo2[t] = State.yServoDegs;
|
||||||
|
|
||||||
stateVector.thrustFiring[t] = State.thrustFiring;
|
stateVector.thrustFiring[t] = State.thrustFiring;
|
||||||
|
}
|
||||||
|
|
||||||
stateVector.PIDx[t] = State.PIDx;
|
void write2CSV(outVector &stateVector, Vehicle &State) {
|
||||||
stateVector.PIDy[t] = State.PIDy;
|
|
||||||
|
|
||||||
stateVector.thrust[t] = State.thrust;
|
// 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"
|
||||||
|
<< std::endl;
|
||||||
|
|
||||||
|
std::cout << "Writing to csv...\n";
|
||||||
|
|
||||||
|
// writing to output file
|
||||||
|
for (int t = 0; t < State.simTime; t++) {
|
||||||
|
outfile << t << ", ";
|
||||||
|
|
||||||
|
outfile << stateVector.x[t] << ", ";
|
||||||
|
outfile << stateVector.y[t] << ", ";
|
||||||
|
outfile << stateVector.z[t] << ", ";
|
||||||
|
|
||||||
|
outfile << stateVector.vx[t] << ", ";
|
||||||
|
outfile << stateVector.vy[t] << ", ";
|
||||||
|
outfile << stateVector.vz[t] << ", ";
|
||||||
|
|
||||||
|
outfile << stateVector.ax[t] << ", ";
|
||||||
|
outfile << stateVector.ay[t] << ", ";
|
||||||
|
outfile << stateVector.az[t] << ", ";
|
||||||
|
|
||||||
|
outfile << stateVector.yaw[t] * 180 / M_PI << ", ";
|
||||||
|
outfile << stateVector.pitch[t] * 180 / M_PI << ", ";
|
||||||
|
outfile << stateVector.roll[t] * 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 << stateVector.servo1[t] << ", ";
|
||||||
|
outfile << stateVector.servo2[t] << ", ";
|
||||||
|
|
||||||
|
outfile << stateVector.thrustFiring[t] << std::endl;
|
||||||
|
}
|
||||||
|
|
||||||
|
outfile.close();
|
||||||
}
|
}
|
||||||
|
|
||||||
double derivative(double current, double previous, double step) {
|
double derivative(double current, double previous, double step) {
|
||||||
double dxdt = (current - previous) / (step / 1000.0);
|
double dxdt = (previous - current) / (step / 1000);
|
||||||
return dxdt;
|
return dxdt;
|
||||||
}
|
}
|
||||||
|
|
||||||
double integral(double currentChange, double prevValue, double dt) {
|
double integral(double currentChange, double prevValue, double dt) {
|
||||||
return (currentChange * dt / 1000.0) + prevValue;
|
return (currentChange * dt / 1000) + prevValue;
|
||||||
}
|
|
||||||
|
|
||||||
double limit(double value, double upr, double lwr) {
|
|
||||||
if (value > upr)
|
|
||||||
value = upr;
|
|
||||||
else if (value < lwr)
|
|
||||||
value = lwr;
|
|
||||||
else
|
|
||||||
value = value;
|
|
||||||
|
|
||||||
return value;
|
|
||||||
}
|
|
||||||
|
|
||||||
void getThrust(Vehicle &State) {
|
|
||||||
if ((State.burnElapsed > 0.147) && (State.burnElapsed < 0.420)) {
|
|
||||||
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.0) -
|
|
||||||
11.609 * pow(State.burnElapsed, 5.0) +
|
|
||||||
60.739 * pow(State.burnElapsed, 4.0) -
|
|
||||||
162.99 * pow(State.burnElapsed, 3.0) +
|
|
||||||
235.6 * pow(State.burnElapsed, 2.0) -
|
|
||||||
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 = 2;
|
|
||||||
State.thrust = 0.0;
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
398
include/teensy.h
398
include/teensy.h
@ -1,398 +0,0 @@
|
|||||||
#include "Vehicle.h"
|
|
||||||
#include <Arduino.h>
|
|
||||||
#include <HX711.h>
|
|
||||||
#include <PWMServo.h>
|
|
||||||
#include <SD.h>
|
|
||||||
#include <SPI.h>
|
|
||||||
|
|
||||||
void testGimbal(class PWMServo &, class PWMServo &);
|
|
||||||
void initLoadCells(struct Vehicle &);
|
|
||||||
void initFile();
|
|
||||||
void thrustInfo(struct Vehicle &);
|
|
||||||
void processTVC(struct Vehicle &, class PWMServo &, class PWMServo &);
|
|
||||||
void write2CSV(struct Vehicle &);
|
|
||||||
void printSimResults(struct Vehicle &);
|
|
||||||
void teensyAbort();
|
|
||||||
double loadCellFilter(double current, double previous);
|
|
||||||
|
|
||||||
// Load cell stuff
|
|
||||||
HX711 lc0;
|
|
||||||
HX711 lc1;
|
|
||||||
HX711 lc2;
|
|
||||||
HX711 lc3;
|
|
||||||
|
|
||||||
void read_lc0();
|
|
||||||
void read_lc1();
|
|
||||||
void read_lc2();
|
|
||||||
void read_lc3();
|
|
||||||
|
|
||||||
const int lc_clock = 23;
|
|
||||||
const int pin_lc0 = 14;
|
|
||||||
const int pin_lc1 = 15;
|
|
||||||
const int pin_lc2 = 19;
|
|
||||||
const int pin_lc3 = 20;
|
|
||||||
const int pin_igniter = 7;
|
|
||||||
|
|
||||||
double lcGain0 = -98.32;
|
|
||||||
double lcGain1 = -97.59;
|
|
||||||
double lcGain2 = -100.51;
|
|
||||||
double lcGain3 = -118.65;
|
|
||||||
|
|
||||||
// SD card stuff
|
|
||||||
const int chipSelect = BUILTIN_SDCARD;
|
|
||||||
File dataFile;
|
|
||||||
|
|
||||||
void testGimbal(PWMServo &servo1, PWMServo &servo2) {
|
|
||||||
int servoTest = 90;
|
|
||||||
|
|
||||||
servo1.write(servoTest);
|
|
||||||
servo2.write(servoTest);
|
|
||||||
|
|
||||||
// Servo 1 Test
|
|
||||||
for (servoTest = 0; servoTest < 7; servoTest += 1) {
|
|
||||||
servo1.write(90 + servoTest);
|
|
||||||
delay(30);
|
|
||||||
}
|
|
||||||
for (servoTest = 7; servoTest >= 1; servoTest -= 1) {
|
|
||||||
servo1.write(90 + servoTest);
|
|
||||||
delay(30);
|
|
||||||
}
|
|
||||||
|
|
||||||
delay(1000);
|
|
||||||
|
|
||||||
// Servo 2 Test
|
|
||||||
for (servoTest = 0; servoTest < 7; servoTest += 1) {
|
|
||||||
servo2.write(90 + servoTest);
|
|
||||||
delay(30);
|
|
||||||
}
|
|
||||||
for (servoTest = 7; servoTest >= 1; servoTest -= 1) {
|
|
||||||
servo2.write(90 + servoTest);
|
|
||||||
delay(30);
|
|
||||||
}
|
|
||||||
|
|
||||||
delay(1000);
|
|
||||||
|
|
||||||
// Servo 1 & 2 Test
|
|
||||||
for (servoTest = 0; servoTest < 7; servoTest += 1) {
|
|
||||||
servo1.write(90 + servoTest);
|
|
||||||
servo2.write(90 + servoTest);
|
|
||||||
delay(30);
|
|
||||||
}
|
|
||||||
for (servoTest = 7; servoTest >= 1; servoTest -= 1) {
|
|
||||||
servo1.write(90 + servoTest);
|
|
||||||
servo2.write(90 + servoTest);
|
|
||||||
delay(30);
|
|
||||||
}
|
|
||||||
|
|
||||||
delay(30);
|
|
||||||
// BRING IN YAW AND PITCH CONVERSION FROM TVC TEST
|
|
||||||
servo1.write(90);
|
|
||||||
servo2.write(90);
|
|
||||||
}
|
|
||||||
|
|
||||||
void initLoadCells(Vehicle &State) {
|
|
||||||
// Configure clock pin with high impedance to protect
|
|
||||||
// pin (if this doesn't work, change to OUTPUT)
|
|
||||||
pinMode(lc_clock, INPUT);
|
|
||||||
|
|
||||||
// Configure load cell data pins as inputs
|
|
||||||
pinMode(pin_lc0, INPUT);
|
|
||||||
pinMode(pin_lc1, INPUT);
|
|
||||||
pinMode(pin_lc2, INPUT);
|
|
||||||
pinMode(pin_lc3, INPUT);
|
|
||||||
lc0.begin(pin_lc0, lc_clock);
|
|
||||||
lc1.begin(pin_lc1, lc_clock);
|
|
||||||
lc2.begin(pin_lc2, lc_clock);
|
|
||||||
lc3.begin(pin_lc3, lc_clock);
|
|
||||||
|
|
||||||
Serial.print("Calibrating");
|
|
||||||
lc0.set_scale();
|
|
||||||
lc1.set_scale();
|
|
||||||
lc2.set_scale();
|
|
||||||
lc3.set_scale();
|
|
||||||
|
|
||||||
lc0.tare(50);
|
|
||||||
Serial.print(".");
|
|
||||||
lc1.tare(50);
|
|
||||||
Serial.print(".");
|
|
||||||
lc2.tare(50);
|
|
||||||
Serial.println(".");
|
|
||||||
lc3.tare(50);
|
|
||||||
|
|
||||||
// Attach ISRs to load cell data pins to read data when available
|
|
||||||
attachInterrupt(digitalPinToInterrupt(pin_lc0), read_lc0, LOW);
|
|
||||||
attachInterrupt(digitalPinToInterrupt(pin_lc1), read_lc1, LOW);
|
|
||||||
attachInterrupt(digitalPinToInterrupt(pin_lc2), read_lc2, LOW);
|
|
||||||
attachInterrupt(digitalPinToInterrupt(pin_lc3), read_lc3, LOW);
|
|
||||||
|
|
||||||
Serial.println("Load Cells Initialized");
|
|
||||||
|
|
||||||
// Initializes State variables
|
|
||||||
State.lc0 = 9.81 * 0.001 * lc0.get_value() / lcGain0;
|
|
||||||
State.lc1 = 9.81 * 0.001 * lc1.get_value() / lcGain1;
|
|
||||||
State.lc2 = 9.81 * 0.001 * lc2.get_value() / lcGain2;
|
|
||||||
State.lc3 = 9.81 * 0.001 * lc3.get_value() / lcGain3;
|
|
||||||
}
|
|
||||||
|
|
||||||
void initFile() {
|
|
||||||
Serial.print("Initializing SD card...");
|
|
||||||
|
|
||||||
// see if the card is present and can be initialized:
|
|
||||||
if (!SD.begin(chipSelect)) {
|
|
||||||
Serial.println("Card failed, or not present. \n\nABORTING SIMULATION");
|
|
||||||
teensyAbort();
|
|
||||||
}
|
|
||||||
Serial.println("Card initialized.");
|
|
||||||
|
|
||||||
int i = 1;
|
|
||||||
const char *fileName;
|
|
||||||
|
|
||||||
if (SD.exists("simOut.csv")) {
|
|
||||||
while (i > 0.0) {
|
|
||||||
fileName = ("simOut_" + String(i) + ".csv").c_str();
|
|
||||||
|
|
||||||
if (!SD.exists(fileName)) {
|
|
||||||
// Open simOut_i.csv
|
|
||||||
dataFile = SD.open(fileName, FILE_WRITE);
|
|
||||||
|
|
||||||
if (dataFile) {
|
|
||||||
Serial.println("File successfully opened!");
|
|
||||||
|
|
||||||
} else {
|
|
||||||
// if the file didn't open, print an error:
|
|
||||||
Serial.println("Error opening output file. \n\nABORTING SIMULATION");
|
|
||||||
teensyAbort();
|
|
||||||
}
|
|
||||||
i = 0.0;
|
|
||||||
|
|
||||||
} else {
|
|
||||||
i++;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
} else {
|
|
||||||
// Open simOut.csv
|
|
||||||
dataFile = SD.open("simOut.csv", FILE_WRITE);
|
|
||||||
if (dataFile) {
|
|
||||||
Serial.println("File successfully opened!");
|
|
||||||
|
|
||||||
} else {
|
|
||||||
// if the file didn't open, print an error:
|
|
||||||
Serial.println("Error opening output file. \n\nABORTING SIMULATION");
|
|
||||||
teensyAbort();
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// File Header
|
|
||||||
dataFile.println(
|
|
||||||
"t,x,y,z,vx,vy,vz,ax,ay,az,yaw,pitch,roll,yawdot,pitchdot,rolldot,"
|
|
||||||
"Servo1,Servo2,thrustFiring,thrust,simResponse,lc0,lc1,lc2,lc3,lc0_"
|
|
||||||
"processed,lc1_processed,"
|
|
||||||
"lc2_processed,lc3_processed");
|
|
||||||
}
|
|
||||||
|
|
||||||
void thrustInfo(Vehicle &State) {
|
|
||||||
if ((abs(State.burnVelocity + State.vz) < 1.03) &&
|
|
||||||
(State.thrustFiring == 0)) {
|
|
||||||
// Start burn
|
|
||||||
State.burnStart = State.time;
|
|
||||||
State.burnElapsed = 0.0;
|
|
||||||
|
|
||||||
analogWrite(pin_igniter, 256);
|
|
||||||
State.thrustFiring = 1;
|
|
||||||
getThrust(State);
|
|
||||||
|
|
||||||
} else if (State.thrustFiring == 1) {
|
|
||||||
State.burnElapsed = (State.time - State.burnStart) / 1000.0;
|
|
||||||
State.mass = State.massInitial - (State.mdot * State.burnElapsed);
|
|
||||||
|
|
||||||
getThrust(State);
|
|
||||||
|
|
||||||
double r = 3.0;
|
|
||||||
double R = 5.0;
|
|
||||||
// Vector math to aqcuire thrust vector components
|
|
||||||
// PLACEHOLDER PLACEHOLDERPLACEHOLDER PLACEHOLDERPLACEHOLDER
|
|
||||||
// PLACEHOLDERPLACEHOLDER PLACEHOLDERPLACEHOLDER PLACEHOLDER
|
|
||||||
State.thrust = State.lc0_processed + State.lc1_processed +
|
|
||||||
State.lc2_processed + State.lc3_processed;
|
|
||||||
State.Fx = (State.lc1_processed - State.lc2_processed) * r / R;
|
|
||||||
State.Fy = (State.lc0_processed - State.lc3_processed) * r / R;
|
|
||||||
State.Fz =
|
|
||||||
sqrt(pow(State.thrust, 2) - pow(State.Fx, 2) - pow(State.Fy, 2)) +
|
|
||||||
(state.mass * g);
|
|
||||||
|
|
||||||
} else {
|
|
||||||
State.thrust = 0.0;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void processTVC(Vehicle &State, PWMServo &servo1, PWMServo &servo2) {
|
|
||||||
State.Fx = State.thrust * sin(State.xServoDegs * (M_PI / 180.0));
|
|
||||||
State.Fy = State.thrust * sin(State.yServoDegs * (M_PI / 180.0));
|
|
||||||
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.0;
|
|
||||||
|
|
||||||
State.F = sqrt(pow(State.Fx, 2) + pow(State.Fy, 2) + pow(State.Fz, 2));
|
|
||||||
|
|
||||||
servo1.write(90 + State.xServoDegs);
|
|
||||||
servo2.write(90 + State.yServoDegs);
|
|
||||||
}
|
|
||||||
|
|
||||||
void write2CSV(Vehicle &State) {
|
|
||||||
dataFile.print(String(State.time, 5));
|
|
||||||
dataFile.print(",");
|
|
||||||
|
|
||||||
dataFile.print(String(State.x, 5));
|
|
||||||
dataFile.print(",");
|
|
||||||
dataFile.print(String(State.y, 5));
|
|
||||||
dataFile.print(",");
|
|
||||||
dataFile.print(String(State.z, 5));
|
|
||||||
dataFile.print(",");
|
|
||||||
|
|
||||||
dataFile.print(String(State.vx, 5));
|
|
||||||
dataFile.print(",");
|
|
||||||
dataFile.print(String(State.vy, 5));
|
|
||||||
dataFile.print(",");
|
|
||||||
dataFile.print(String(State.vz, 5));
|
|
||||||
dataFile.print(",");
|
|
||||||
|
|
||||||
dataFile.print(String(State.ax, 5));
|
|
||||||
dataFile.print(",");
|
|
||||||
dataFile.print(String(State.ay, 5));
|
|
||||||
dataFile.print(",");
|
|
||||||
dataFile.print(String(State.az, 5));
|
|
||||||
dataFile.print(",");
|
|
||||||
|
|
||||||
dataFile.print(String(State.yaw * 180.0 / M_PI, 5));
|
|
||||||
dataFile.print(",");
|
|
||||||
dataFile.print(String(State.pitch * 180.0 / M_PI, 5));
|
|
||||||
dataFile.print(",");
|
|
||||||
dataFile.print(String(State.roll * 180.0 / M_PI, 5));
|
|
||||||
dataFile.print(",");
|
|
||||||
|
|
||||||
dataFile.print(String(State.yawdot * 180.0 / M_PI, 5));
|
|
||||||
dataFile.print(",");
|
|
||||||
dataFile.print(String(State.pitchdot * 180.0 / M_PI, 5));
|
|
||||||
dataFile.print(",");
|
|
||||||
dataFile.print(String(State.rolldot * 180.0 / M_PI, 5));
|
|
||||||
dataFile.print(",");
|
|
||||||
|
|
||||||
dataFile.print(String(State.xServoDegs, 5));
|
|
||||||
dataFile.print(",");
|
|
||||||
dataFile.print(String(State.yServoDegs, 5));
|
|
||||||
dataFile.print(",");
|
|
||||||
|
|
||||||
dataFile.print(String(State.thrustFiring, 5));
|
|
||||||
dataFile.print(",");
|
|
||||||
dataFile.print(String(State.thrust, 5));
|
|
||||||
dataFile.print(",");
|
|
||||||
|
|
||||||
dataFile.print(String(State.stepDuration, 5));
|
|
||||||
dataFile.print(",");
|
|
||||||
|
|
||||||
dataFile.print(String(State.lc0, 5));
|
|
||||||
dataFile.print(",");
|
|
||||||
dataFile.print(String(State.lc1, 5));
|
|
||||||
dataFile.print(",");
|
|
||||||
dataFile.print(String(State.lc2, 5));
|
|
||||||
dataFile.print(",");
|
|
||||||
dataFile.print(String(State.lc3, 5));
|
|
||||||
dataFile.print(",");
|
|
||||||
|
|
||||||
dataFile.print(String(State.lc0_processed, 5));
|
|
||||||
dataFile.print(",");
|
|
||||||
dataFile.print(String(State.lc1_processed, 5));
|
|
||||||
dataFile.print(",");
|
|
||||||
dataFile.print(String(State.lc2_processed, 5));
|
|
||||||
dataFile.print(",");
|
|
||||||
dataFile.print(String(State.lc3_processed, 5));
|
|
||||||
|
|
||||||
dataFile.print("\n");
|
|
||||||
}
|
|
||||||
|
|
||||||
void printSimResults(Vehicle &State) {
|
|
||||||
State.yaw = State.yaw * 180.0 / M_PI;
|
|
||||||
State.pitch = State.pitch * 180.0 / M_PI;
|
|
||||||
State.roll = State.roll * 180.0 / M_PI;
|
|
||||||
|
|
||||||
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) {
|
|
||||||
Serial.print("Landing Angle < 5.0 degrees | PASS | ");
|
|
||||||
} else {
|
|
||||||
Serial.print("Landing Angle < 5.0 degrees | FAIL | ");
|
|
||||||
}
|
|
||||||
Serial.print("Final Angles: [" + String(State.yaw) + ", " +
|
|
||||||
String(State.pitch) + "]\n");
|
|
||||||
|
|
||||||
if (landing_velocity < 1.0) {
|
|
||||||
Serial.print("Landing Velocity < 1.0 m/s | PASS | ");
|
|
||||||
} else {
|
|
||||||
Serial.print("Landing Velocity < 1.0 m/s | FAIL | ");
|
|
||||||
}
|
|
||||||
Serial.print("Final Velocity: [" + String(State.vx) + ", " +
|
|
||||||
String(State.vy) + ", " + String(State.vz) + "]\n");
|
|
||||||
|
|
||||||
Serial.print("\nSimulation Complete\n");
|
|
||||||
}
|
|
||||||
|
|
||||||
void closeFile() {
|
|
||||||
// close the file:
|
|
||||||
dataFile.close();
|
|
||||||
Serial.println("File closed\n");
|
|
||||||
}
|
|
||||||
|
|
||||||
void teensyAbort() {
|
|
||||||
while (1) {
|
|
||||||
digitalWrite(BUILTIN_LED, HIGH);
|
|
||||||
delay(1000);
|
|
||||||
digitalWrite(BUILTIN_LED, LOW);
|
|
||||||
delay(1000);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// ISRs to print data to serial monitor
|
|
||||||
void read_lc0() {
|
|
||||||
State.lc0 = lc0.get_value();
|
|
||||||
State.lc0_processed = 0.00981 * State.lc0 / lcGain0;
|
|
||||||
State.lc0_processed =
|
|
||||||
loadCellFilter(State.lc0_processed, PrevState.lc0_processed);
|
|
||||||
PrevState.lc0_processed = State.lc0_processed;
|
|
||||||
}
|
|
||||||
void read_lc1() {
|
|
||||||
State.lc1 = lc1.get_value();
|
|
||||||
State.lc1_processed = 0.00981 * State.lc1 / lcGain1;
|
|
||||||
State.lc1_processed =
|
|
||||||
loadCellFilter(State.lc1_processed, PrevState.lc1_processed);
|
|
||||||
PrevState.lc1_processed = State.lc1_processed;
|
|
||||||
}
|
|
||||||
void read_lc2() {
|
|
||||||
State.lc2 = lc2.get_value();
|
|
||||||
State.lc2_processed = 0.00981 * State.lc2 / lcGain2;
|
|
||||||
State.lc2_processed =
|
|
||||||
loadCellFilter(State.lc2_processed, PrevState.lc2_processed);
|
|
||||||
PrevState.lc2_processed = State.lc2_processed;
|
|
||||||
}
|
|
||||||
void read_lc3() {
|
|
||||||
State.lc3 = lc3.get_value();
|
|
||||||
State.lc3_processed = 0.00981 * State.lc3 / lcGain3;
|
|
||||||
State.lc3_processed =
|
|
||||||
loadCellFilter(State.lc3_processed, PrevState.lc3_processed);
|
|
||||||
PrevState.lc3_processed = State.lc3_processed;
|
|
||||||
}
|
|
||||||
|
|
||||||
double loadCellFilter(double current, double previous) {
|
|
||||||
if (abs(current - previous > 60.0)) {
|
|
||||||
return previous;
|
|
||||||
} else {
|
|
||||||
return current;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
File diff suppressed because it is too large
Load Diff
@ -1,5 +0,0 @@
|
|||||||
[deps]
|
|
||||||
CSV = "336ed68f-0bac-5ca0-87d4-7b16caf5d00b"
|
|
||||||
DataFrames = "a93c6f00-e57d-5684-b7b6-d8193f3e46c0"
|
|
||||||
PlotlyJS = "f0f68f2c-4968-5e81-91da-67840de0976a"
|
|
||||||
Plots = "91a5bcdd-55d7-5caf-9e0b-520d859cae80"
|
|
||||||
@ -1,45 +0,0 @@
|
|||||||
using Plots
|
|
||||||
using CSV
|
|
||||||
using DataFrames
|
|
||||||
|
|
||||||
theme(:juno)
|
|
||||||
plotlyjs()
|
|
||||||
|
|
||||||
|
|
||||||
df = CSV.File("./public/simOut.csv") |> DataFrame
|
|
||||||
# df = CSV.File("./simOut.csv") |> DataFrame
|
|
||||||
|
|
||||||
|
|
||||||
println(describe(df))
|
|
||||||
|
|
||||||
p1 = let
|
|
||||||
a = plot(df.t, [df.ax df.ay df.az], labels = ["ẍ" "ÿ" "z̈"], title = "Acceleration", ylabel = "m/s^2")
|
|
||||||
|
|
||||||
v = plot(df.t, df.vx .* df.vx .+ df.vy .* df.vy .+ df.vz .* df.vz .|> sqrt, labels = "Total Velocity", title = "Total Velocity", ylabel = "m/s", linecolor = "orchid")
|
|
||||||
|
|
||||||
x = plot(df.t, df.z, labels = "Altitude", title = "Altitude", ylabel = "m", xlabel = "time (ms)", linecolor = "chartreuse")
|
|
||||||
|
|
||||||
|
|
||||||
plot(a, v, x, layout = (3, 1), legend = :outertopright)
|
|
||||||
end
|
|
||||||
|
|
||||||
|
|
||||||
p2 = let
|
|
||||||
ypr = plot(df.t, [df.yaw df.pitch df.roll], label = ["Yaw" "Pitch" "Roll"], ylabel = "Euler Angles (degree)", title = "Vehicle Deflection")
|
|
||||||
|
|
||||||
ω = plot(df.t, [df.yawdot df.pitchdot df.rolldot], label = ["ÿ" "p̈" "r̈"], ylabel = "Angular Velocity (deg/s)", title = "Vehicle Deflection", xlabel = "time (ms)")
|
|
||||||
|
|
||||||
plot(ypr, ω, layout = (2, 1))
|
|
||||||
end
|
|
||||||
|
|
||||||
p3 = plot(df.t, [df.Servo1 df.Servo2], label = ["Yaw" "Pitch"], ylabel = "Servo Position (degree)", xlabel = "time (ms)", title = "Servo Positions")
|
|
||||||
|
|
||||||
|
|
||||||
global i = 1
|
|
||||||
theme(:ggplot2)
|
|
||||||
for p in [p1 p2 p3]
|
|
||||||
savefig(p, "./public/plot$i.svg")
|
|
||||||
savefig(p, "./public/plot$i.html")
|
|
||||||
savefig(p, "./public/plot$i.png")
|
|
||||||
global i = i + 1
|
|
||||||
end
|
|
||||||
@ -36,7 +36,7 @@ S = icare(A, B, Q, R);
|
|||||||
K = Rinv * B' * S;
|
K = Rinv * B' * S;
|
||||||
|
|
||||||
%% Outputs
|
%% Outputs
|
||||||
% Copy results in command window to PIDcalc function in C++
|
% Copy results in command window to LQRcalc function in C++
|
||||||
fprintf("double K11 = %3.5f;\n", K(1, 1))
|
fprintf("double K11 = %3.5f;\n", K(1, 1))
|
||||||
fprintf("double K12 = %3.5f;\n", K(1, 2))
|
fprintf("double K12 = %3.5f;\n", K(1, 2))
|
||||||
fprintf("double K13 = %3.5f;\n", K(1, 3))
|
fprintf("double K13 = %3.5f;\n", K(1, 3))
|
||||||
|
|||||||
@ -27,10 +27,6 @@ rolldot = T(:, 16);
|
|||||||
Servo1 = T(:, 17);
|
Servo1 = T(:, 17);
|
||||||
Servo2 = T(:, 18);
|
Servo2 = T(:, 18);
|
||||||
|
|
||||||
PIDx = T(:, 20);
|
|
||||||
PIDy = T(:, 21);
|
|
||||||
|
|
||||||
thrust = T(:, 22);
|
|
||||||
% Acceleration
|
% Acceleration
|
||||||
subplot(3, 1, 1)
|
subplot(3, 1, 1)
|
||||||
plot(t, az)
|
plot(t, az)
|
||||||
@ -52,6 +48,7 @@ title('Altitude vs Time')
|
|||||||
xlabel('Time (s)')
|
xlabel('Time (s)')
|
||||||
ylabel('Altitude (m)')
|
ylabel('Altitude (m)')
|
||||||
ylim([0 z(1)+5])
|
ylim([0 z(1)+5])
|
||||||
|
saveas(gcf,'outputs/Accel-Vel-Alt vs Time.png')
|
||||||
|
|
||||||
figure(2)
|
figure(2)
|
||||||
|
|
||||||
@ -75,6 +72,7 @@ plot(t, rolldot)
|
|||||||
title('Angular Velocity vs Time')
|
title('Angular Velocity vs Time')
|
||||||
xlabel('Time (ms)')
|
xlabel('Time (ms)')
|
||||||
ylabel('Angular Velocity (deg/s)')
|
ylabel('Angular Velocity (deg/s)')
|
||||||
|
saveas(gcf,'outputs/Euler Angles vs Time.png')
|
||||||
legend("yawdot", "pitchdot", "rolldot")
|
legend("yawdot", "pitchdot", "rolldot")
|
||||||
|
|
||||||
figure(3)
|
figure(3)
|
||||||
@ -92,12 +90,4 @@ plot(t, Servo2)
|
|||||||
title('Servo 2 Position vs Time')
|
title('Servo 2 Position vs Time')
|
||||||
xlabel('Time (ms)')
|
xlabel('Time (ms)')
|
||||||
ylabel('Servo 2 Position (rad)')
|
ylabel('Servo 2 Position (rad)')
|
||||||
|
saveas(gcf,'outputs/Servo Position vs Time.png')
|
||||||
figure(4)
|
|
||||||
|
|
||||||
% Servo 1 Position
|
|
||||||
|
|
||||||
plot(t, thrust)
|
|
||||||
title('Thrust vs Time')
|
|
||||||
xlabel('Time (ms)')
|
|
||||||
ylabel('Thrust (N)')
|
|
||||||
|
|||||||
@ -1,20 +0,0 @@
|
|||||||
; PlatformIO Project Configuration File
|
|
||||||
;
|
|
||||||
; Build options: build flags, source filter
|
|
||||||
; Upload options: custom upload port, speed and extra flags
|
|
||||||
; Library options: dependencies, extra library storages
|
|
||||||
; Advanced options: extra scripting
|
|
||||||
;
|
|
||||||
; Please visit documentation for the other options and examples
|
|
||||||
; https://docs.platformio.org/page/projectconf.html
|
|
||||||
|
|
||||||
[env:NATIVE]
|
|
||||||
platform = native
|
|
||||||
build_flags = -std=c++11 -Wall -O1 -D NATIVE
|
|
||||||
|
|
||||||
[env:teensy41]
|
|
||||||
platform = teensy
|
|
||||||
board = teensy41
|
|
||||||
framework = arduino
|
|
||||||
build_flags = -std=c++11 -Wall -O1 -D TEENSY
|
|
||||||
lib_deps = bogde/HX711@^0.7.4
|
|
||||||
@ -4,7 +4,7 @@ import pandas as pd
|
|||||||
|
|
||||||
plt.style.use("ggplot")
|
plt.style.use("ggplot")
|
||||||
|
|
||||||
df = pd.read_csv("./public/simOut.csv")
|
df = pd.read_csv("./public/simOut.csv", delimiter=", ")
|
||||||
|
|
||||||
|
|
||||||
# Acceleration
|
# Acceleration
|
||||||
|
|||||||
170
src/main.cpp
170
src/main.cpp
@ -1,139 +1,67 @@
|
|||||||
#if defined(_WIN32) || defined(NATIVE)
|
#define M_PI 3.14159265359
|
||||||
|
|
||||||
#include <cmath>
|
#include <cmath>
|
||||||
#include <fstream>
|
#include <fstream>
|
||||||
#include <iostream>
|
#include <iostream>
|
||||||
#include <stdexcept> // std::runtime_error
|
|
||||||
#include <stdio.h>
|
#include <stdio.h>
|
||||||
#include <string>
|
|
||||||
#include <vector>
|
|
||||||
#elif defined(TEENSY)
|
|
||||||
#include <Arduino.h>
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#define M_PI 3.14159265359
|
|
||||||
#include "Vehicle.h"
|
#include "Vehicle.h"
|
||||||
#include "sim.h"
|
#include "sim.h"
|
||||||
|
|
||||||
Vehicle State;
|
bool sim(struct Vehicle &);
|
||||||
Vehicle PrevState;
|
|
||||||
|
|
||||||
#if defined(TEENSY)
|
|
||||||
|
|
||||||
int BUILTIN_LED = 13;
|
|
||||||
unsigned long last, initTime;
|
|
||||||
|
|
||||||
#include "teensy.h"
|
|
||||||
#include <HX711.h>
|
|
||||||
#include <PWMServo.h>
|
|
||||||
|
|
||||||
PWMServo servo1;
|
|
||||||
PWMServo servo2;
|
|
||||||
|
|
||||||
const int pin_servo1 = 33;
|
|
||||||
const int pin_servo2 = 29;
|
|
||||||
const int pin_igniter = 7;
|
|
||||||
|
|
||||||
int servoPos = 90; // variable to store the servo position;
|
|
||||||
|
|
||||||
void setup() {
|
|
||||||
Serial.begin(9600);
|
|
||||||
|
|
||||||
delay(5000);
|
|
||||||
Serial.println("Simulation Countdown:");
|
|
||||||
for (int i = 0; i < 10; i++) {
|
|
||||||
Serial.println(10 - i);
|
|
||||||
delay(1000);
|
|
||||||
}
|
|
||||||
|
|
||||||
initLoadCells(State);
|
|
||||||
init_Vehicle(State);
|
|
||||||
|
|
||||||
servo1.attach(pin_servo1);
|
|
||||||
servo2.attach(pin_servo2);
|
|
||||||
testGimbal(servo1, servo2);
|
|
||||||
Serial.println("Servos Tested");
|
|
||||||
|
|
||||||
delay(3000);
|
|
||||||
Serial.println("Simulated Vehicle Initalized");
|
|
||||||
|
|
||||||
// Determine when to burn
|
|
||||||
analogWrite(pin_igniter, 0);
|
|
||||||
burnStartTimeCalc(State);
|
|
||||||
Serial.println("Starting Height Calculated");
|
|
||||||
|
|
||||||
initFile();
|
|
||||||
|
|
||||||
// PLACE BUTTON HERE---------------------------------------------
|
|
||||||
|
|
||||||
initTime = micros();
|
|
||||||
}
|
|
||||||
|
|
||||||
void loop() {
|
|
||||||
last = micros();
|
|
||||||
|
|
||||||
vehicleDynamics(State, PrevState);
|
|
||||||
thrustInfo(State);
|
|
||||||
pidController(State, PrevState);
|
|
||||||
TVC(State, PrevState);
|
|
||||||
processTVC(State, servo1, servo2);
|
|
||||||
|
|
||||||
write2CSV(State);
|
|
||||||
|
|
||||||
// Set previous values for next timestep
|
|
||||||
PrevState = State;
|
|
||||||
State.time += State.stepSize;
|
|
||||||
|
|
||||||
if ((State.z < 0.0) && (State.thrustFiring == 2)) {
|
|
||||||
analogWrite(pin_igniter, 0);
|
|
||||||
Serial.println("Run duration:" + String((micros() - initTime) / 1000000.0) +
|
|
||||||
" seconds");
|
|
||||||
printSimResults(State);
|
|
||||||
|
|
||||||
closeFile();
|
|
||||||
delay(3000);
|
|
||||||
|
|
||||||
Serial.println("SUCCESS");
|
|
||||||
Serial.println("Exiting Sim");
|
|
||||||
teensyAbort();
|
|
||||||
}
|
|
||||||
|
|
||||||
State.stepDuration = micros() - last;
|
|
||||||
delayMicroseconds((1000.0 * State.stepSize) - State.stepDuration);
|
|
||||||
}
|
|
||||||
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#if defined(_WIN32) || defined(NATIVE)
|
|
||||||
|
|
||||||
#include "native.h"
|
|
||||||
outVector stateVector;
|
|
||||||
|
|
||||||
int main() {
|
int main() {
|
||||||
|
Vehicle State;
|
||||||
|
Vehicle PrevState;
|
||||||
|
|
||||||
init_Vehicle(State);
|
// Initial Velocity
|
||||||
|
State.vx = 0; // [m/s]
|
||||||
|
State.vy = 0; // [m/s]
|
||||||
|
State.vz = 0; // [m/s]
|
||||||
|
|
||||||
// Determine when to burn
|
// Initial YPR
|
||||||
burnStartTimeCalc(State);
|
State.yaw = 10 * M_PI / 180; // [rad]
|
||||||
|
State.pitch = 5 * M_PI / 180; // [rad]
|
||||||
|
State.roll = 0 * M_PI / 180; // [rad]
|
||||||
|
|
||||||
do {
|
// Initial YPRdot
|
||||||
vehicleDynamics(State, PrevState);
|
State.yawdot = 1 * M_PI / 180; // [rad/s]
|
||||||
thrustInfo(State);
|
State.pitchdot = -1 * M_PI / 180; // [rad/s]
|
||||||
pidController(State, PrevState);
|
State.rolldot = 0 * M_PI / 180; // [rad/s]
|
||||||
TVC(State, PrevState);
|
|
||||||
processTVC(State);
|
|
||||||
state2vec(State, PrevState, stateVector);
|
|
||||||
|
|
||||||
// Set "prev" values for next timestep
|
// Servo Limitation
|
||||||
PrevState = State;
|
State.maxServo = 15; // [degs]
|
||||||
State.time += State.stepSize;
|
|
||||||
|
|
||||||
if ((State.z < 0.0) && (State.thrustFiring == 2)) {
|
// Vehicle Properties
|
||||||
write2CSV(stateVector, State);
|
State.massInitial = 1.2; // [kg]
|
||||||
printSimResults(State);
|
State.vehicleHeight = 0.5318; // [m]
|
||||||
init_Vehicle(State);
|
State.vehicleRadius = 0.05105; // [m]
|
||||||
}
|
State.momentArm = 0.145; // [m]
|
||||||
} while ((State.z > 0.0) || (State.thrustFiring != 2));
|
|
||||||
|
|
||||||
|
// 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 << "Finished"
|
||||||
|
<< "\n";
|
||||||
|
|
||||||
|
if (outcome == 1) {
|
||||||
|
std::cout << "Sim Result = Success!";
|
||||||
return 0;
|
return 0;
|
||||||
|
} else if (outcome == 0) {
|
||||||
|
std::cout << "Sim Result = Failed!";
|
||||||
|
// return 1; Until I figure out how to make CI/CD continue even when run
|
||||||
|
// fails.
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
#endif
|
|
||||||
|
|||||||
Loading…
x
Reference in New Issue
Block a user