Anson/Lander-Embedded
1
0
Fork 0
mirror of https://gitlab.com/lander-team/lander-cpp.git synced 2025-08-17 02:24:50 +00:00
Code Releases Activity

Merge branch 'main' into '22-make-function-to-turn-test-stand-load-cells-into-thrust-vector'

Browse Source 
# Conflicts:
#   .vscode/extensions.json
#   include/teensy.h
#   src/main.cpp
This commit is contained in:
Anson Biggs
2021-11-12 20:46:35 +00:00
parent 2a0426334d 8ec17dd820
commit f7c34b6c8c
9 changed files with 285 additions and 138 deletions
Download Patch File Download Diff File Expand all files Collapse all files

194
include/teensy.h
View File

@@ -1,49 +1,105 @@
#include "Vehicle.h"
#include <Arduino.h>
#include <SD.h>
#include <SPI.h>
double loadCellCalibrate();
void initFile();
void thrustInfo(struct Vehicle &);
void processTVC(struct LoadCells &);
void write2CSV(struct outVector &, struct Vehicle &);
void processTVC(struct Vehicle &);
void write2CSV(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) {
if (State.time == 0) {
Serial.println("WARNING: thrustInfo not implemented for TEENSY");
}
if (State.burnElapsed != 2000) {
// determine where in the thrust curve we're at based on elapsed burn time
// as well as current mass
State.burnElapsed = (State.time - State.burnStart) / 1000;
State.mass = State.massInitial - (State.mdot * State.burnElapsed);
}
else if (abs(State.burnVelocity + State.vz) < 0.001) {
if ((abs(State.burnVelocity + State.vz) < 1.03) &&
(State.thrustFiring == 0)) {
// Start burn
State.burnStart = State.time;
State.burnElapsed = 0;
}
State.burnElapsed = 0.0;
State.thrustFiring = 1;
else
State.burnElapsed = 2000; // arbitrary number to ensure we don't burn
getThrust(State);
if ((State.burnElapsed > 0.147) && (State.burnElapsed < 0.420)) {
State.thrustFiring = true;
State.thrust = 65.165 * State.burnElapsed - 2.3921;
} else if (State.thrustFiring == 1) {
State.burnElapsed = (State.time - State.burnStart) / 1000.0;
State.mass = State.massInitial - (State.mdot * State.burnElapsed);
} 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;
getThrust(State);
else if ((State.burnElapsed > 3.382) && (State.burnElapsed < 3.46))
State.thrust = -195.78 * State.burnElapsed - 675.11;
if (State.burnElapsed > 3.45) {
State.thrustFiring = false;
State.thrust = 0;
} else {
State.thrust = 0.0;
}
}
@@ -63,15 +119,70 @@ void processTVC(Vehicle &State, LoadCells &loadCells) {
// Calculate moment created by Fx and Fy
State.momentX = State.Fx * 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) {
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);
@@ -94,5 +205,20 @@ void printSimResults(Vehicle &State) {
Serial.print("Final Velocity: [" + String(State.vx) + ", " +
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);
}
}