mirror of
https://gitlab.com/lander-team/lander-cpp.git
synced 2025-06-16 15:17:23 +00:00
328 lines
8.6 KiB
C
328 lines
8.6 KiB
C
#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 init_loadCells(class HX711 &, class HX711 &, class HX711 &, class HX711 &);
|
|
void initFile();
|
|
void thrustInfo(struct Vehicle &);
|
|
void processTVC(struct Vehicle &, class PWMServo &, class PWMServo &);
|
|
void write2CSV(struct Vehicle &);
|
|
void printSimResults(struct Vehicle &);
|
|
void teensyAbort();
|
|
|
|
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 chipSelect = BUILTIN_SDCARD;
|
|
File dataFile;
|
|
|
|
void testGimbal(PWMServo &servo1, PWMServo &servo2) {
|
|
int servoTest = 0;
|
|
|
|
servo1.write(servoTest);
|
|
servo2.write(servoTest);
|
|
|
|
// Servo 1 Test
|
|
for (servoTest = 0; servoTest < 7; servoTest += 1) {
|
|
servo1.write(servoTest);
|
|
delay(30);
|
|
}
|
|
for (servoTest = 7; servoTest >= 1; servoTest -= 1) {
|
|
servo1.write(servoTest);
|
|
delay(30);
|
|
}
|
|
|
|
delay(1000);
|
|
|
|
// Servo 2 Test
|
|
for (servoTest = 0; servoTest < 7; servoTest += 1) {
|
|
servo2.write(servoTest);
|
|
delay(30);
|
|
}
|
|
for (servoTest = 7; servoTest >= 1; servoTest -= 1) {
|
|
servo2.write(servoTest);
|
|
delay(30);
|
|
}
|
|
|
|
delay(1000);
|
|
|
|
// Servo 1 & 2 Test
|
|
for (servoTest = 0; servoTest < 7; servoTest += 1) {
|
|
servo1.write(servoTest);
|
|
servo2.write(servoTest);
|
|
delay(30);
|
|
}
|
|
for (servoTest = 7; servoTest >= 1; servoTest -= 1) {
|
|
servo1.write(servoTest);
|
|
servo2.write(servoTest);
|
|
delay(30);
|
|
}
|
|
|
|
delay(30);
|
|
servo1.write(0);
|
|
servo2.write(0);
|
|
}
|
|
|
|
void init_loadCells(HX711 &lc0, HX711 &lc1, HX711 &lc2, HX711 &lc3) {
|
|
// 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);
|
|
|
|
// 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");
|
|
|
|
Serial.print("Calibrating");
|
|
lc0.tare();
|
|
Serial.print(".");
|
|
lc1.tare();
|
|
Serial.print(".");
|
|
lc2.tare();
|
|
Serial.print(".");
|
|
lc3.tare();
|
|
Serial.println(".");
|
|
Serial.println("Load Cells Calibrated");
|
|
}
|
|
|
|
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");
|
|
}
|
|
|
|
void thrustInfo(Vehicle &State) {
|
|
if (State.time == 0) {
|
|
Serial.println("WARNING: thrustInfo not implemented for TEENSY");
|
|
}
|
|
|
|
if ((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, PWMServo &servo1, PWMServo &servo2) {
|
|
if (State.time == 0) {
|
|
Serial.println("WARNING: processTVC not implemented for TEENSY");
|
|
}
|
|
double r = 3.0;
|
|
double R = 5.0;
|
|
// Vector math to aqcuire thrust vector components
|
|
// PLACEHOLDER PLACEHOLDERPLACEHOLDER PLACEHOLDERPLACEHOLDER
|
|
// PLACEHOLDERPLACEHOLDER PLACEHOLDERPLACEHOLDER PLACEHOLDER
|
|
State.Fx = (State.lc1 - State.lc2) * r / R;
|
|
State.Fy = (State.lc0 - State.lc3) * r / R;
|
|
State.Fz = State.lc0 + State.lc1 + State.lc2 + State.lc3;
|
|
|
|
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(State.xServoDegs);
|
|
servo2.write(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("\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);
|
|
}
|
|
} |