mirror of
https://gitlab.com/lander-team/lander-cpp.git
synced 2025-06-16 15:17:23 +00:00
Merge branch 'main' into '22-make-function-to-turn-test-stand-load-cells-into-thrust-vector'
# Conflicts: # .vscode/extensions.json # include/teensy.h # src/main.cpp
This commit is contained in:
commit
f7c34b6c8c
3
.vscode/settings.json
vendored
3
.vscode/settings.json
vendored
@ -45,7 +45,8 @@
|
|||||||
"string": "cpp",
|
"string": "cpp",
|
||||||
"chrono": "cpp",
|
"chrono": "cpp",
|
||||||
"ratio": "cpp",
|
"ratio": "cpp",
|
||||||
"thread": "cpp"
|
"thread": "cpp",
|
||||||
|
"string_view": "cpp"
|
||||||
},
|
},
|
||||||
"C_Cpp.clang_format_fallbackStyle": "LLVM",
|
"C_Cpp.clang_format_fallbackStyle": "LLVM",
|
||||||
"editor.formatOnSave": true,
|
"editor.formatOnSave": true,
|
||||||
|
@ -20,7 +20,7 @@ struct Vehicle {
|
|||||||
double burntime;
|
double burntime;
|
||||||
double burnVelocity;
|
double burnVelocity;
|
||||||
double thrust, burnElapsed, burnStart;
|
double thrust, burnElapsed, burnStart;
|
||||||
bool thrustFiring = false;
|
int thrustFiring = 0; // 0: Pre-burn, 1: Mid-burn, 2: Post-burn
|
||||||
|
|
||||||
double PIDx, PIDy, Fx, Fy, Fz;
|
double PIDx, PIDy, Fx, Fy, Fz;
|
||||||
double momentX, momentY, momentZ;
|
double momentX, momentY, momentZ;
|
||||||
@ -28,47 +28,47 @@ struct Vehicle {
|
|||||||
double I11, I22, I33;
|
double I11, I22, I33;
|
||||||
double I11dot, I22dot, I33dot;
|
double I11dot, I22dot, I33dot;
|
||||||
|
|
||||||
int maxServo;
|
double maxServo;
|
||||||
int maxServoRate;
|
double maxServoRate;
|
||||||
double xServoDegs, yServoDegs;
|
double xServoDegs, yServoDegs;
|
||||||
double xServoDegsDot, yServoDegsDot;
|
double xServoDegsDot, yServoDegsDot;
|
||||||
|
|
||||||
double Kp, Ki, Kd;
|
double Kp, Ki, Kd;
|
||||||
double yError, yPrevError;
|
double yError, yPrevError;
|
||||||
double pError, pPrevError;
|
double pError, pPrevError;
|
||||||
double i_yError, i_pError = 0;
|
double i_yError, i_pError = 0.0;
|
||||||
double d_yError, d_pError;
|
double d_yError, d_pError;
|
||||||
|
|
||||||
double simTime;
|
double simTime;
|
||||||
int stepSize;
|
double stepSize;
|
||||||
|
|
||||||
int time = 0;
|
double time = 0.0;
|
||||||
};
|
};
|
||||||
|
|
||||||
void init_Vehicle(Vehicle &State) {
|
void init_Vehicle(Vehicle &State) {
|
||||||
// PID Gains
|
// PID Gains
|
||||||
State.Kp = -6.8699;
|
State.Kp = -6.8699;
|
||||||
State.Ki = 0;
|
State.Ki = 0.0;
|
||||||
State.Kd = -0.775;
|
State.Kd = -0.775;
|
||||||
|
|
||||||
// Initial Velocity
|
// Initial Velocity
|
||||||
State.vx = 0; // [m/s]
|
State.vx = 0.0; // [m/s]
|
||||||
State.vy = 0; // [m/s]
|
State.vy = 0.0; // [m/s]
|
||||||
State.vz = 0; // [m/s]
|
State.vz = 0.0; // [m/s]
|
||||||
|
|
||||||
// Initial YPR
|
// Initial YPR
|
||||||
State.yaw = 45 * M_PI / 180; // [rad]
|
State.yaw = 45.0 * M_PI / 180.0; // [rad]
|
||||||
State.pitch = 45 * M_PI / 180; // [rad]
|
State.pitch = 45.0 * M_PI / 180.0; // [rad]
|
||||||
State.roll = 0 * M_PI / 180; // [rad]
|
State.roll = 0.0 * M_PI / 180.0; // [rad]
|
||||||
|
|
||||||
// Initial YPRdot
|
// Initial YPRdot
|
||||||
State.yawdot = 1 * M_PI / 180; // [rad/s]
|
State.yawdot = 1.0 * M_PI / 180.0; // [rad/s]
|
||||||
State.pitchdot = -1 * M_PI / 180; // [rad/s]
|
State.pitchdot = -1.0 * M_PI / 180.0; // [rad/s]
|
||||||
State.rolldot = 0 * M_PI / 180; // [rad/s]
|
State.rolldot = 0.0 * M_PI / 180.0; // [rad/s]
|
||||||
|
|
||||||
// Servo Limitation
|
// Servo Limitation
|
||||||
State.maxServo = 7; // [degs]
|
State.maxServo = 7.0; // [degs]
|
||||||
State.maxServoRate = 360; // [degs/sec]
|
State.maxServoRate = 360.0; // [degs/sec]
|
||||||
|
|
||||||
// Vehicle Properties
|
// Vehicle Properties
|
||||||
State.massInitial = 1.2; // [kg]
|
State.massInitial = 1.2; // [kg]
|
||||||
@ -77,7 +77,7 @@ void init_Vehicle(Vehicle &State) {
|
|||||||
State.momentArm = 0.145; // [m]
|
State.momentArm = 0.145; // [m]
|
||||||
|
|
||||||
// Sim Step Size
|
// Sim Step Size
|
||||||
State.stepSize = 1; // [ms]
|
State.stepSize = 1.0; // [ms]
|
||||||
|
|
||||||
// Other Properties
|
// Other Properties
|
||||||
State.massPropellant = 0.06; // [kg]
|
State.massPropellant = 0.06; // [kg]
|
||||||
|
@ -8,41 +8,23 @@ void write2CSV(struct outVector &, struct Vehicle &);
|
|||||||
void printSimResults(struct Vehicle &);
|
void printSimResults(struct Vehicle &);
|
||||||
|
|
||||||
void thrustInfo(Vehicle &State) {
|
void thrustInfo(Vehicle &State) {
|
||||||
|
if ((std::abs(State.burnVelocity + State.vz) < 1.03) &&
|
||||||
if (State.burnElapsed != 2000) {
|
(State.thrustFiring == 0)) {
|
||||||
// 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
|
// Start burn
|
||||||
State.burnStart = State.time;
|
State.burnStart = State.time;
|
||||||
State.burnElapsed = 0;
|
State.burnElapsed = 0.0;
|
||||||
}
|
State.thrustFiring = 1;
|
||||||
|
|
||||||
else
|
getThrust(State);
|
||||||
State.burnElapsed = 2000; // arbitrary number to ensure we don't burn
|
|
||||||
|
|
||||||
if ((State.burnElapsed > 0.147) && (State.burnElapsed < 0.420)) {
|
} else if (State.thrustFiring == 1) {
|
||||||
State.thrustFiring = true;
|
State.burnElapsed = (State.time - State.burnStart) / 1000.0;
|
||||||
State.thrust = 65.165 * State.burnElapsed - 2.3921;
|
State.mass = State.massInitial - (State.mdot * State.burnElapsed);
|
||||||
|
|
||||||
} else if ((State.burnElapsed > 0.419) && (State.burnElapsed < 3.383))
|
getThrust(State);
|
||||||
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))
|
} else {
|
||||||
State.thrust = -195.78 * State.burnElapsed - 675.11;
|
State.thrust = 0.0;
|
||||||
|
|
||||||
if (State.burnElapsed > 3.45) {
|
|
||||||
State.thrustFiring = false;
|
|
||||||
State.thrust = 0;
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -117,6 +99,10 @@ void write2CSV(outVector &stateVector, Vehicle &State) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
void printSimResults(Vehicle &State) {
|
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 =
|
double landing_angle =
|
||||||
pow(State.yaw * State.yaw + State.pitch * State.pitch, .5);
|
pow(State.yaw * State.yaw + State.pitch * State.pitch, .5);
|
||||||
|
|
||||||
|
@ -4,11 +4,7 @@
|
|||||||
#define OUTVECTOR_H
|
#define OUTVECTOR_H
|
||||||
|
|
||||||
struct outVector {
|
struct outVector {
|
||||||
#if defined(NATIVE) || defined(_WIN32)
|
int length = 10000; // current sim runs ~5000 steps, x2 just in case
|
||||||
int length = 100000; // current sim runs ~5000 steps, x2 just in case
|
|
||||||
#elif defined(TEENSY)
|
|
||||||
int length = 1000; // current sim runs ~5000 steps, x2 just in case
|
|
||||||
#endif
|
|
||||||
|
|
||||||
std::vector<double> x = std::vector<double>(length, 0.0);
|
std::vector<double> x = std::vector<double>(length, 0.0);
|
||||||
std::vector<double> y = std::vector<double>(length, 0.0);
|
std::vector<double> y = std::vector<double>(length, 0.0);
|
||||||
|
@ -11,6 +11,7 @@ void state2vec(struct Vehicle &, struct Vehicle &, struct outVector &);
|
|||||||
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);
|
||||||
|
void getThrust(struct Vehicle &);
|
||||||
|
|
||||||
// Any parameters that are constants should be declared here instead of
|
// Any parameters that are constants should be declared here instead of
|
||||||
// buried in code
|
// buried in code
|
||||||
@ -19,7 +20,7 @@ double const g = -9.81;
|
|||||||
|
|
||||||
void burnStartTimeCalc(Vehicle &State) {
|
void burnStartTimeCalc(Vehicle &State) {
|
||||||
double velocity = State.vz;
|
double velocity = State.vz;
|
||||||
double h = 0;
|
double h = 0.0;
|
||||||
|
|
||||||
double mass, thrust;
|
double mass, thrust;
|
||||||
|
|
||||||
@ -37,39 +38,39 @@ 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
|
else
|
||||||
thrust = 0;
|
thrust = 0.0;
|
||||||
|
|
||||||
velocity = (((thrust / mass) + g) * dt) + velocity;
|
velocity = (((thrust / mass) + g) * dt) + velocity;
|
||||||
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.0 * -g)); // starting height
|
||||||
State.z = 18.9;
|
State.z = 19.05;
|
||||||
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;
|
State.simTime = (State.burntime + burnStartTime) * 1000.0;
|
||||||
}
|
}
|
||||||
|
|
||||||
void vehicleDynamics(Vehicle &State, Vehicle &PrevState) {
|
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.0) * pow(State.vehicleHeight, 2) +
|
||||||
pow(State.vehicleRadius, 2) / 4);
|
pow(State.vehicleRadius, 2) / 4.0);
|
||||||
State.I22 = State.mass * ((1 / 12) * pow(State.vehicleHeight, 2) +
|
State.I22 = State.mass * ((1 / 12.0) * pow(State.vehicleHeight, 2) +
|
||||||
pow(State.vehicleRadius, 2) / 4);
|
pow(State.vehicleRadius, 2) / 4.0);
|
||||||
State.I33 = State.mass * 0.5 * pow(State.vehicleRadius, 2);
|
State.I33 = State.mass * 0.5 * pow(State.vehicleRadius, 2);
|
||||||
|
|
||||||
// Idot
|
// Idot
|
||||||
if (State.time < 0.1) {
|
if (State.time < 0.1) {
|
||||||
State.I11dot = 0;
|
State.I11dot = 0.0;
|
||||||
State.I22dot = 0;
|
State.I22dot = 0.0;
|
||||||
State.I33dot = 0;
|
State.I33dot = 0.0;
|
||||||
|
|
||||||
State.x = 0;
|
State.x = 0.0;
|
||||||
State.y = 0;
|
State.y = 0.0;
|
||||||
|
|
||||||
State.ax = 0;
|
State.ax = 0.0;
|
||||||
State.ay = 0;
|
State.ay = 0.0;
|
||||||
State.az = State.Fz / State.massInitial;
|
State.az = State.Fz / State.massInitial;
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
@ -145,7 +146,6 @@ void pidController(Vehicle &State, struct Vehicle &PrevState) {
|
|||||||
State.d_pError = derivative(State.pError, PrevState.pError, State.stepSize);
|
State.d_pError = derivative(State.pError, PrevState.pError, State.stepSize);
|
||||||
|
|
||||||
// TVC block properly
|
// TVC block properly
|
||||||
|
|
||||||
State.PIDx = (State.Kp * State.yError + State.Ki * State.i_yError +
|
State.PIDx = (State.Kp * State.yError + State.Ki * State.i_yError +
|
||||||
State.Kd * State.d_yError) /
|
State.Kd * State.d_yError) /
|
||||||
State.momentArm;
|
State.momentArm;
|
||||||
@ -154,8 +154,8 @@ void pidController(Vehicle &State, struct Vehicle &PrevState) {
|
|||||||
State.momentArm;
|
State.momentArm;
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
State.PIDx = 0;
|
State.PIDx = 0.0;
|
||||||
State.PIDy = 0;
|
State.PIDy = 0.0;
|
||||||
}
|
}
|
||||||
|
|
||||||
// PID Force Limiter
|
// PID Force Limiter
|
||||||
@ -166,18 +166,18 @@ void pidController(Vehicle &State, struct Vehicle &PrevState) {
|
|||||||
void TVC(Vehicle &State, Vehicle &PrevState) {
|
void TVC(Vehicle &State, Vehicle &PrevState) {
|
||||||
if (State.thrust < 0.1) {
|
if (State.thrust < 0.1) {
|
||||||
// Define forces and moments for t = 0
|
// Define forces and moments for t = 0
|
||||||
State.Fx = 0;
|
State.Fx = 0.0;
|
||||||
State.Fy = 0;
|
State.Fy = 0.0;
|
||||||
State.Fz = g * State.massInitial;
|
State.Fz = g * State.massInitial;
|
||||||
|
|
||||||
State.momentX = 0;
|
State.momentX = 0.0;
|
||||||
State.momentY = 0;
|
State.momentY = 0.0;
|
||||||
State.momentZ = 0;
|
State.momentZ = 0.0;
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
// Convert servo position to degrees for comparison to max allowable
|
// Convert servo position to degrees for comparison to max allowable
|
||||||
State.xServoDegs = (180 / M_PI) * asin(State.PIDx / State.thrust);
|
State.xServoDegs = (180.0 / M_PI) * asin(State.PIDx / State.thrust);
|
||||||
State.yServoDegs = (180 / M_PI) * asin(State.PIDy / State.thrust);
|
State.yServoDegs = (180.0 / M_PI) * asin(State.PIDy / State.thrust);
|
||||||
|
|
||||||
// Limit Servo Position
|
// Limit Servo Position
|
||||||
State.xServoDegs = limit(State.xServoDegs, State.maxServo, -State.maxServo);
|
State.xServoDegs = limit(State.xServoDegs, State.maxServo, -State.maxServo);
|
||||||
@ -235,18 +235,15 @@ void state2vec(Vehicle &State, Vehicle &PrevState, outVector &stateVector) {
|
|||||||
stateVector.PIDy[t] = State.PIDy;
|
stateVector.PIDy[t] = State.PIDy;
|
||||||
|
|
||||||
stateVector.thrust[t] = State.thrust;
|
stateVector.thrust[t] = State.thrust;
|
||||||
|
|
||||||
// Set "prev" values for next timestep
|
|
||||||
PrevState = State;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
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.0);
|
||||||
return dxdt;
|
return dxdt;
|
||||||
}
|
}
|
||||||
|
|
||||||
double integral(double currentChange, double prevValue, double dt) {
|
double integral(double currentChange, double prevValue, double dt) {
|
||||||
return (currentChange * dt / 1000) + prevValue;
|
return (currentChange * dt / 1000.0) + prevValue;
|
||||||
}
|
}
|
||||||
|
|
||||||
double limit(double value, double upr, double lwr) {
|
double limit(double value, double upr, double lwr) {
|
||||||
@ -259,3 +256,25 @@ double limit(double value, double upr, double lwr) {
|
|||||||
|
|
||||||
return 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;
|
||||||
|
}
|
||||||
|
}
|
194
include/teensy.h
194
include/teensy.h
@ -1,49 +1,105 @@
|
|||||||
#include "Vehicle.h"
|
#include "Vehicle.h"
|
||||||
|
#include <Arduino.h>
|
||||||
|
#include <SD.h>
|
||||||
|
#include <SPI.h>
|
||||||
|
|
||||||
|
double loadCellCalibrate();
|
||||||
|
void initFile();
|
||||||
void thrustInfo(struct Vehicle &);
|
void thrustInfo(struct Vehicle &);
|
||||||
void processTVC(struct LoadCells &);
|
void processTVC(struct Vehicle &);
|
||||||
void write2CSV(struct outVector &, struct Vehicle &);
|
void write2CSV(struct Vehicle &);
|
||||||
void printSimResults(struct Vehicle &);
|
void printSimResults(struct Vehicle &);
|
||||||
|
void teensyAbort();
|
||||||
|
|
||||||
|
const int chipSelect = BUILTIN_SDCARD;
|
||||||
|
File dataFile;
|
||||||
|
|
||||||
|
double loadCellCalibrate() {
|
||||||
|
// place code to calibrate load cells in here
|
||||||
|
double loadTotal;
|
||||||
|
for (double t = 0.0; t == 10.0; ++t) {
|
||||||
|
loadTotal += 1.0;
|
||||||
|
delay(15);
|
||||||
|
}
|
||||||
|
return loadTotal / 10.0;
|
||||||
|
}
|
||||||
|
|
||||||
|
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,PIDx,PIDy,thrust");
|
||||||
|
}
|
||||||
|
|
||||||
void thrustInfo(Vehicle &State) {
|
void thrustInfo(Vehicle &State) {
|
||||||
if (State.time == 0) {
|
if (State.time == 0) {
|
||||||
Serial.println("WARNING: thrustInfo not implemented for TEENSY");
|
Serial.println("WARNING: thrustInfo not implemented for TEENSY");
|
||||||
}
|
}
|
||||||
|
|
||||||
if (State.burnElapsed != 2000) {
|
if ((abs(State.burnVelocity + State.vz) < 1.03) &&
|
||||||
// determine where in the thrust curve we're at based on elapsed burn time
|
(State.thrustFiring == 0)) {
|
||||||
// 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
|
// Start burn
|
||||||
State.burnStart = State.time;
|
State.burnStart = State.time;
|
||||||
State.burnElapsed = 0;
|
State.burnElapsed = 0.0;
|
||||||
}
|
State.thrustFiring = 1;
|
||||||
|
|
||||||
else
|
getThrust(State);
|
||||||
State.burnElapsed = 2000; // arbitrary number to ensure we don't burn
|
|
||||||
|
|
||||||
if ((State.burnElapsed > 0.147) && (State.burnElapsed < 0.420)) {
|
} else if (State.thrustFiring == 1) {
|
||||||
State.thrustFiring = true;
|
State.burnElapsed = (State.time - State.burnStart) / 1000.0;
|
||||||
State.thrust = 65.165 * State.burnElapsed - 2.3921;
|
State.mass = State.massInitial - (State.mdot * State.burnElapsed);
|
||||||
|
|
||||||
} else if ((State.burnElapsed > 0.419) && (State.burnElapsed < 3.383))
|
getThrust(State);
|
||||||
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))
|
} else {
|
||||||
State.thrust = -195.78 * State.burnElapsed - 675.11;
|
State.thrust = 0.0;
|
||||||
|
|
||||||
if (State.burnElapsed > 3.45) {
|
|
||||||
State.thrustFiring = false;
|
|
||||||
State.thrust = 0;
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -63,15 +119,70 @@ void processTVC(Vehicle &State, LoadCells &loadCells) {
|
|||||||
// Calculate moment created by Fx and Fy
|
// Calculate moment created by Fx and Fy
|
||||||
State.momentX = State.Fx * State.momentArm;
|
State.momentX = State.Fx * State.momentArm;
|
||||||
State.momentY = State.Fy * State.momentArm;
|
State.momentY = State.Fy * State.momentArm;
|
||||||
State.momentZ = 0;
|
State.momentZ = 0.0;
|
||||||
}
|
}
|
||||||
|
|
||||||
void write2CSV(outVector &stateVector, Vehicle &State) {
|
void write2CSV(Vehicle &State) {
|
||||||
|
dataFile.print(String(State.time, 5));
|
||||||
|
dataFile.print(",");
|
||||||
|
|
||||||
Serial.println("WARNING: write2CSV not implemented for TEENSY");
|
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.PIDx, 5));
|
||||||
|
dataFile.print(",");
|
||||||
|
dataFile.print(String(State.PIDy, 5));
|
||||||
|
dataFile.print(",");
|
||||||
|
|
||||||
|
dataFile.print(String(State.thrust, 5));
|
||||||
|
dataFile.print("\n");
|
||||||
}
|
}
|
||||||
|
|
||||||
void printSimResults(Vehicle &State) {
|
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 =
|
double landing_angle =
|
||||||
pow(State.yaw * State.yaw + State.pitch * State.pitch, .5);
|
pow(State.yaw * State.yaw + State.pitch * State.pitch, .5);
|
||||||
|
|
||||||
@ -94,5 +205,20 @@ void printSimResults(Vehicle &State) {
|
|||||||
Serial.print("Final Velocity: [" + String(State.vx) + ", " +
|
Serial.print("Final Velocity: [" + String(State.vx) + ", " +
|
||||||
String(State.vy) + ", " + String(State.vz) + "]\n");
|
String(State.vy) + ", " + String(State.vz) + "]\n");
|
||||||
|
|
||||||
Serial.print("\n\nSimulation Complete\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);
|
||||||
|
}
|
||||||
}
|
}
|
@ -30,6 +30,7 @@ Servo2 = T(:, 18);
|
|||||||
PIDx = T(:, 20);
|
PIDx = T(:, 20);
|
||||||
PIDy = T(:, 21);
|
PIDy = T(:, 21);
|
||||||
|
|
||||||
|
thrust = T(:, 22);
|
||||||
% Acceleration
|
% Acceleration
|
||||||
subplot(3, 1, 1)
|
subplot(3, 1, 1)
|
||||||
plot(t, az)
|
plot(t, az)
|
||||||
@ -91,3 +92,12 @@ 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)')
|
||||||
|
|
||||||
|
figure(4)
|
||||||
|
|
||||||
|
% Servo 1 Position
|
||||||
|
|
||||||
|
plot(t, thrust)
|
||||||
|
title('Thrust vs Time')
|
||||||
|
xlabel('Time (ms)')
|
||||||
|
ylabel('Thrust (N)')
|
||||||
|
27
src/main.cpp
27
src/main.cpp
@ -10,6 +10,8 @@
|
|||||||
#include <vector>
|
#include <vector>
|
||||||
#elif defined(TEENSY)
|
#elif defined(TEENSY)
|
||||||
#include <Arduino.h>
|
#include <Arduino.h>
|
||||||
|
|
||||||
|
int BUILTIN_LED = 13;
|
||||||
unsigned long last;
|
unsigned long last;
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
@ -18,6 +20,7 @@ unsigned long last;
|
|||||||
|
|
||||||
#if defined(NATIVE) || defined(_WIN32)
|
#if defined(NATIVE) || defined(_WIN32)
|
||||||
#include "native.h"
|
#include "native.h"
|
||||||
|
outVector stateVector;
|
||||||
#elif defined(TEENSY)
|
#elif defined(TEENSY)
|
||||||
|
|
||||||
#include "LoadCells.h"
|
#include "LoadCells.h"
|
||||||
@ -35,7 +38,6 @@ const int lc_data_3 = 3;
|
|||||||
|
|
||||||
Vehicle State;
|
Vehicle State;
|
||||||
Vehicle PrevState;
|
Vehicle PrevState;
|
||||||
outVector stateVector;
|
|
||||||
|
|
||||||
#if defined(NATIVE) || defined(_WIN32)
|
#if defined(NATIVE) || defined(_WIN32)
|
||||||
void setup() {
|
void setup() {
|
||||||
@ -71,6 +73,8 @@ void setup() {
|
|||||||
|
|
||||||
Serial.println("Load Cells Calibrated");
|
Serial.println("Load Cells Calibrated");
|
||||||
delay(1000);
|
delay(1000);
|
||||||
|
initFile();
|
||||||
|
delay(1000);
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
@ -83,9 +87,11 @@ void loop() {
|
|||||||
processTVC(State);
|
processTVC(State);
|
||||||
state2vec(State, PrevState, stateVector);
|
state2vec(State, PrevState, stateVector);
|
||||||
|
|
||||||
|
// Set "prev" values for next timestep
|
||||||
|
PrevState = State;
|
||||||
State.time += State.stepSize;
|
State.time += State.stepSize;
|
||||||
|
|
||||||
if (State.z < 0.0) {
|
if ((State.z < 0.0) && (State.thrustFiring == 2)) {
|
||||||
write2CSV(stateVector, State);
|
write2CSV(stateVector, State);
|
||||||
printSimResults(State);
|
printSimResults(State);
|
||||||
init_Vehicle(State);
|
init_Vehicle(State);
|
||||||
@ -93,6 +99,7 @@ void loop() {
|
|||||||
}
|
}
|
||||||
#elif defined(TEENSY)
|
#elif defined(TEENSY)
|
||||||
void loop() {
|
void loop() {
|
||||||
|
|
||||||
last = millis();
|
last = millis();
|
||||||
vehicleDynamics(State, PrevState);
|
vehicleDynamics(State, PrevState);
|
||||||
thrustInfo(State);
|
thrustInfo(State);
|
||||||
@ -103,14 +110,16 @@ void loop() {
|
|||||||
|
|
||||||
State.time += State.stepSize;
|
State.time += State.stepSize;
|
||||||
|
|
||||||
if (State.z < 0.0) {
|
if ((State.z < 0.0) && (State.thrustFiring == 2)) {
|
||||||
write2CSV(stateVector, State);
|
|
||||||
printSimResults(State);
|
printSimResults(State);
|
||||||
init_Vehicle(State);
|
Serial.println("Run duration:" + String(millis() - last) + " ms");
|
||||||
Serial.println("Last run duration:" + String(millis() - last + " ms"));
|
|
||||||
|
|
||||||
delay(1000);
|
closeFile();
|
||||||
Serial.println("Restarting Sim");
|
delay(20000);
|
||||||
|
|
||||||
|
Serial.println("SUCCESS");
|
||||||
|
Serial.println("Aborting Sim");
|
||||||
|
teensyAbort();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
@ -122,7 +131,7 @@ int main() {
|
|||||||
|
|
||||||
do {
|
do {
|
||||||
loop();
|
loop();
|
||||||
} while ((State.z > 0.0));
|
} while ((State.z > 0.0) || (State.thrustFiring != 2));
|
||||||
|
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
Loading…
x
Reference in New Issue
Block a user