Anson/Lander-Embedded
1
0
Fork 0
mirror of https://gitlab.com/lander-team/lander-cpp.git synced 2025-06-16 07:06:51 +00:00
Code Releases Activity

Merge branch '27-fix-simulation-on-teensy' into 'main'

Browse Source 
Resolve "Fix SImulation on Teensy"

Closes #27

See merge request lander-team/lander-cpp!23
This commit is contained in:
Brendan McGeeney 2021-11-12 19:26:58 +00:00
commit 8ec17dd820
6 changed files with 140 additions and 146 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
include/Vehicle.h
View File

@ -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]

40
include/native.h
View File

@ -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;
} }
} }

78
include/sim.h
View File

@ -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);
@ -238,12 +238,12 @@ void state2vec(Vehicle &State, Vehicle &PrevState, outVector &stateVector) {
} }
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) {
@ -255,4 +255,26 @@ double limit(double value, double upr, double lwr) {
value = value; value = value;
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;
}
} }

108
include/teensy.h
View File

@ -17,11 +17,11 @@ File dataFile;
double loadCellCalibrate() { double loadCellCalibrate() {
// place code to calibrate load cells in here // place code to calibrate load cells in here
double loadTotal; double loadTotal;
for (double t = 0; t == 10; ++t) { for (double t = 0.0; t == 10.0; ++t) {
loadTotal += 1; loadTotal += 1.0;
delay(15); delay(15);
} }
return loadTotal / 10; return loadTotal / 10.0;
} }
void initFile() { void initFile() {
@ -35,11 +35,10 @@ void initFile() {
Serial.println("Card initialized."); Serial.println("Card initialized.");
int i = 1; int i = 1;
Serial.print("simOut_" + String(i) + ".csv");
const char *fileName; const char *fileName;
if (SD.exists("simOut.csv")) { if (SD.exists("simOut.csv")) {
while (i > 0) { while (i > 0.0) {
fileName = ("simOut_" + String(i) + ".csv").c_str(); fileName = ("simOut_" + String(i) + ".csv").c_str();
if (!SD.exists(fileName)) { if (!SD.exists(fileName)) {
@ -54,7 +53,7 @@ void initFile() {
Serial.println("Error opening output file. \n\nABORTING SIMULATION"); Serial.println("Error opening output file. \n\nABORTING SIMULATION");
teensyAbort(); teensyAbort();
} }
i = 0; i = 0.0;
} else { } else {
i++; i++;
@ -84,120 +83,103 @@ void thrustInfo(Vehicle &State) {
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;
} }
} }
void processTVC(Vehicle &State) { void processTVC(Vehicle &State) {
if (State.time == 0) { if (State.time == 0.0) {
Serial.println("WARNING: processTVC not implemented for TEENSY"); Serial.println("WARNING: processTVC not implemented for TEENSY");
} }
// Vector math to aqcuire thrust vector components // Vector math to aqcuire thrust vector components
State.Fx = State.thrust * sin(State.xServoDegs * (M_PI / 180)); State.Fx = State.thrust * sin(State.xServoDegs * (M_PI / 180.0));
State.Fy = State.thrust * sin(State.yServoDegs * (M_PI / 180)); 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.Fz = sqrt(pow(State.thrust, 2) - pow(State.Fx, 2) - pow(State.Fy, 2)) +
(State.mass * g); (State.mass * g);
// 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(Vehicle &State) { void write2CSV(Vehicle &State) {
dataFile.print(State.time); dataFile.print(String(State.time, 5));
dataFile.print(","); dataFile.print(",");
dataFile.print(State.x); dataFile.print(String(State.x, 5));
dataFile.print(","); dataFile.print(",");
dataFile.print(State.y); dataFile.print(String(State.y, 5));
dataFile.print(","); dataFile.print(",");
dataFile.print(State.z); dataFile.print(String(State.z, 5));
dataFile.print(","); dataFile.print(",");
dataFile.print(State.vx); dataFile.print(String(State.vx, 5));
dataFile.print(","); dataFile.print(",");
dataFile.print(State.vy); dataFile.print(String(State.vy, 5));
dataFile.print(","); dataFile.print(",");
dataFile.print(State.vz); dataFile.print(String(State.vz, 5));
dataFile.print(","); dataFile.print(",");
dataFile.print(State.ax); dataFile.print(String(State.ax, 5));
dataFile.print(","); dataFile.print(",");
dataFile.print(State.ay); dataFile.print(String(State.ay, 5));
dataFile.print(","); dataFile.print(",");
dataFile.print(State.az); dataFile.print(String(State.az, 5));
dataFile.print(","); dataFile.print(",");
dataFile.print(State.yaw * 180 / M_PI); dataFile.print(String(State.yaw * 180.0 / M_PI, 5));
dataFile.print(","); dataFile.print(",");
dataFile.print(State.pitch * 180 / M_PI); dataFile.print(String(State.pitch * 180.0 / M_PI, 5));
dataFile.print(","); dataFile.print(",");
dataFile.print(State.roll * 180 / M_PI); dataFile.print(String(State.roll * 180.0 / M_PI, 5));
dataFile.print(","); dataFile.print(",");
dataFile.print(State.yawdot * 180 / M_PI); dataFile.print(String(State.yawdot * 180.0 / M_PI, 5));
dataFile.print(","); dataFile.print(",");
dataFile.print(State.pitchdot * 180 / M_PI); dataFile.print(String(State.pitchdot * 180.0 / M_PI, 5));
dataFile.print(","); dataFile.print(",");
dataFile.print(State.rolldot * 180 / M_PI); dataFile.print(String(State.rolldot * 180.0 / M_PI, 5));
dataFile.print(","); dataFile.print(",");
dataFile.print(State.xServoDegs); dataFile.print(String(State.xServoDegs, 5));
dataFile.print(","); dataFile.print(",");
dataFile.print(State.yServoDegs); dataFile.print(String(State.yServoDegs, 5));
dataFile.print(","); dataFile.print(",");
dataFile.print(State.thrustFiring); dataFile.print(String(State.thrustFiring, 5));
dataFile.print(","); dataFile.print(",");
dataFile.print(State.PIDx); dataFile.print(String(State.PIDx, 5));
dataFile.print(","); dataFile.print(",");
dataFile.print(State.PIDy); dataFile.print(String(State.PIDy, 5));
dataFile.print(","); dataFile.print(",");
dataFile.print(State.thrust); dataFile.print(String(State.thrust, 5));
dataFile.print("\n"); dataFile.print("\n");
} }
void printSimResults(Vehicle &State) { void printSimResults(Vehicle &State) {
State.yaw = State.yaw * 180 / M_PI; State.yaw = State.yaw * 180.0 / M_PI;
State.pitch = State.pitch * 180 / M_PI; State.pitch = State.pitch * 180.0 / M_PI;
State.roll = State.roll * 180 / 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);

10
matlabHelpers/simPlot.m
View File

@ -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)')

12
src/main.cpp
View File

@ -37,9 +37,7 @@ void setup() {
} }
#elif defined(TEENSY) #elif defined(TEENSY)
void setup() { void setup() {
pinMode(BUILTIN_LED, OUTPUT); delay(5000);
digitalWrite(BUILTIN_LED, HIGH);
delay(1000);
init_Vehicle(State); init_Vehicle(State);
Serial.println("Simulated Vehicle Initalized"); Serial.println("Simulated Vehicle Initalized");
delay(1000); delay(1000);
@ -67,10 +65,9 @@ void loop() {
// Set "prev" values for next timestep // Set "prev" values for next timestep
PrevState = State; 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);
@ -92,11 +89,12 @@ void loop() {
State.time += State.stepSize; State.time += State.stepSize;
if (State.z < 0.0) { if ((State.z < 0.0) && (State.thrustFiring == 2)) {
printSimResults(State); printSimResults(State);
Serial.println("Run duration:" + String(millis() - last) + " ms"); Serial.println("Run duration:" + String(millis() - last) + " ms");
closeFile(); closeFile();
delay(20000);
Serial.println("SUCCESS"); Serial.println("SUCCESS");
Serial.println("Aborting Sim"); Serial.println("Aborting Sim");
@ -112,7 +110,7 @@ int main() {
do { do {
loop(); loop();
} while ((State.z > 0.0)); } while ((State.z > 0.0) || (State.thrustFiring != 2));
return 0; return 0;
} }