mirror of
https://gitlab.com/lander-team/lander-cpp.git
synced 2025-06-16 15:17:23 +00:00
415 lines
11 KiB
C
415 lines
11 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 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);
|
|
float yaw_conv(float thetad);
|
|
float pitch_conv(float thetad);
|
|
|
|
// 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 = 15; // Green
|
|
const int pin_lc1 = 14; // Yellow
|
|
const int pin_lc2 = 19; // White
|
|
const int pin_lc3 = 20; // Black
|
|
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 &yaw, PWMServo &pitch) {
|
|
int servoTest = 90;
|
|
|
|
yaw.write(yaw_conv(servoTest));
|
|
pitch.write(pitch_conv(servoTest));
|
|
|
|
// Servo 1 Test
|
|
for (servoTest = 0; servoTest < 7; servoTest += 1) {
|
|
yaw.write(yaw_conv(servoTest));
|
|
delay(30);
|
|
}
|
|
for (servoTest = 7; servoTest >= 1; servoTest -= 1) {
|
|
yaw.write(yaw_conv(servoTest));
|
|
delay(30);
|
|
}
|
|
|
|
delay(1000);
|
|
|
|
// Servo 2 Test
|
|
for (servoTest = 0; servoTest < 7; servoTest += 1) {
|
|
pitch.write(pitch_conv(servoTest));
|
|
delay(30);
|
|
}
|
|
for (servoTest = 7; servoTest >= 1; servoTest -= 1) {
|
|
pitch.write(pitch_conv(servoTest));
|
|
delay(30);
|
|
}
|
|
|
|
delay(1000);
|
|
|
|
// Servo 1 & 2 Test
|
|
for (servoTest = 0; servoTest < 7; servoTest += 1) {
|
|
yaw.write(yaw_conv(servoTest));
|
|
pitch.write(pitch_conv(servoTest));
|
|
delay(30);
|
|
}
|
|
for (servoTest = 7; servoTest >= 1; servoTest -= 1) {
|
|
yaw.write(yaw_conv(servoTest));
|
|
pitch.write(pitch_conv(servoTest));
|
|
delay(30);
|
|
}
|
|
|
|
delay(30);
|
|
// BRING IN YAW AND PITCH CONVERSION FROM TVC TEST
|
|
yaw.write(yaw_conv(90));
|
|
pitch.write(pitch_conv(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 = lc0.get_value();
|
|
State.lc1 = lc1.get_value();
|
|
State.lc2 = lc2.get_value();
|
|
State.lc3 = lc3.get_value();
|
|
|
|
State.lc0_processed = 9.81 * 0.001 * lc0.get_value() / lcGain0;
|
|
State.lc1_processed = 9.81 * 0.001 * lc1.get_value() / lcGain1;
|
|
State.lc2_processed = 9.81 * 0.001 * lc2.get_value() / lcGain2;
|
|
State.lc3_processed = 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);
|
|
|
|
// Constants based on vehicle
|
|
double r = 3.0;
|
|
double R = 5.0;
|
|
|
|
// Vector math to aqcuire thrust vector components
|
|
State.Fz = State.lc0_processed + State.lc1_processed + State.lc2_processed +
|
|
State.lc3_processed + (State.mass * g);
|
|
State.Fx = (State.lc1_processed - State.lc2_processed) * r / R;
|
|
State.Fy = (State.lc0_processed - State.lc3_processed) * r / R;
|
|
State.thrust = sqrt(pow(State.Fz, 2) + pow(State.Fx, 2) + pow(State.Fy, 2));
|
|
|
|
} else {
|
|
State.thrust = 0.0;
|
|
}
|
|
}
|
|
|
|
void processTVC(Vehicle &State, PWMServo &yaw, PWMServo &pitch) {
|
|
/*
|
|
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);
|
|
*/
|
|
|
|
yaw.write(yaw_conv(State.xServoDegs));
|
|
pitch.write(pitch_conv(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;
|
|
}
|
|
}
|
|
|
|
float yaw_conv(float thetad) {
|
|
float h = .2875;
|
|
float H = 1.6;
|
|
|
|
float theta = thetad * M_PI / 180.0;
|
|
|
|
return 90 + (2 * asin((H * sin(theta / 2)) / h)) * 180.0 / M_PI;
|
|
}
|
|
float pitch_conv(float thetad) {
|
|
float h = .2875;
|
|
float H = 1.2;
|
|
|
|
float theta = thetad * M_PI / 180.0;
|
|
|
|
return 90 + (2 * asin((H * sin(theta / 2)) / h)) * 180.0 / M_PI;
|
|
} |