Anson/Lander-Embedded
1
0
Fork 0
mirror of https://gitlab.com/lander-team/lander-cpp.git synced 2025-07-23 14:41:25 +00:00
Code Releases Activity

Resolve "Move init_loadCells() to teensy.h"

Browse Source
This commit is contained in:
Brendan McGeeney
2021-11-22 19:55:19 +00:00
committed by Anson Biggs
parent 8054e26857
commit 9442884d44
3 changed files with 126 additions and 71 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
include/Vehicle.h
View File

@@ -45,7 +45,8 @@ struct Vehicle {
double time = 0.0;
double lc0, lc1, lc2, lc3;
double lc0, lc1, lc2, lc3 = 0.0;
double lc0_processed, lc1_processed, lc2_processed, lc3_processed = 0.0;
};
void init_Vehicle(Vehicle &State) {

158
include/teensy.h
View File

@@ -6,13 +6,20 @@
#include <SPI.h>
void testGimbal(class PWMServo &, class PWMServo &);
void init_loadCells(class HX711 &, class HX711 &, class HX711 &, class HX711 &);
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);
// Load cell stuff
HX711 lc0;
HX711 lc1;
HX711 lc2;
HX711 lc3;
void read_lc0();
void read_lc1();
@@ -20,28 +27,34 @@ 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 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 &servo1, PWMServo &servo2) {
int servoTest = 0;
int servoTest = 90;
servo1.write(servoTest);
servo2.write(servoTest);
// Servo 1 Test
for (servoTest = 0; servoTest < 7; servoTest += 1) {
servo1.write(servoTest);
servo1.write(90 + servoTest);
delay(30);
}
for (servoTest = 7; servoTest >= 1; servoTest -= 1) {
servo1.write(servoTest);
servo1.write(90 + servoTest);
delay(30);
}
@@ -49,11 +62,11 @@ void testGimbal(PWMServo &servo1, PWMServo &servo2) {
// Servo 2 Test
for (servoTest = 0; servoTest < 7; servoTest += 1) {
servo2.write(servoTest);
servo2.write(90 + servoTest);
delay(30);
}
for (servoTest = 7; servoTest >= 1; servoTest -= 1) {
servo2.write(servoTest);
servo2.write(90 + servoTest);
delay(30);
}
@@ -61,22 +74,23 @@ void testGimbal(PWMServo &servo1, PWMServo &servo2) {
// Servo 1 & 2 Test
for (servoTest = 0; servoTest < 7; servoTest += 1) {
servo1.write(servoTest);
servo2.write(servoTest);
servo1.write(90 + servoTest);
servo2.write(90 + servoTest);
delay(30);
}
for (servoTest = 7; servoTest >= 1; servoTest -= 1) {
servo1.write(servoTest);
servo2.write(servoTest);
servo1.write(90 + servoTest);
servo2.write(90 + servoTest);
delay(30);
}
delay(30);
servo1.write(0);
servo2.write(0);
// BRING IN YAW AND PITCH CONVERSION FROM TVC TEST
servo1.write(90);
servo2.write(90);
}
void init_loadCells(HX711 &lc0, HX711 &lc1, HX711 &lc2, HX711 &lc3) {
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);
@@ -91,6 +105,20 @@ void init_loadCells(HX711 &lc0, HX711 &lc1, HX711 &lc2, HX711 &lc3) {
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);
@@ -99,16 +127,11 @@ void init_loadCells(HX711 &lc0, HX711 &lc1, HX711 &lc2, HX711 &lc3) {
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");
// Initializes State variables
State.lc0 = 9.81 * 0.001 * lc0.get_value() / lcGain0;
State.lc1 = 9.81 * 0.001 * lc1.get_value() / lcGain1;
State.lc2 = 9.81 * 0.001 * lc2.get_value() / lcGain2;
State.lc3 = 9.81 * 0.001 * lc3.get_value() / lcGain3;
}
void initFile() {
@@ -162,21 +185,20 @@ void initFile() {
// 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");
"Servo1,Servo2,thrustFiring,thrust,simResponse,lc0,lc1,lc2,lc3,lc0_"
"processed,lc1_processed,"
"lc2_processed,lc3_processed");
}
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;
analogWrite(pin_igniter, 256);
State.thrustFiring = 1;
getThrust(State);
} else if (State.thrustFiring == 1) {
@@ -185,24 +207,25 @@ void thrustInfo(Vehicle &State) {
getThrust(State);
double r = 3.0;
double R = 5.0;
// Vector math to aqcuire thrust vector components
// PLACEHOLDER PLACEHOLDERPLACEHOLDER PLACEHOLDERPLACEHOLDER
// PLACEHOLDERPLACEHOLDER PLACEHOLDERPLACEHOLDER PLACEHOLDER
State.thrust = State.lc0_processed + State.lc1_processed +
State.lc2_processed + State.lc3_processed;
State.Fx = (State.lc1_processed - State.lc2_processed) * r / R;
State.Fy = (State.lc0_processed - State.lc3_processed) * r / R;
State.Fz =
sqrt(pow(State.thrust, 2) - pow(State.Fx, 2) - pow(State.Fy, 2)) +
(state.mass * g);
} 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)) +
@@ -215,8 +238,8 @@ void processTVC(Vehicle &State, PWMServo &servo1, PWMServo &servo2) {
State.F = sqrt(pow(State.Fx, 2) + pow(State.Fy, 2) + pow(State.Fz, 2));
servo1.write(State.xServoDegs);
servo2.write(State.yServoDegs);
servo1.write(90 + State.xServoDegs);
servo2.write(90 + State.yServoDegs);
}
void write2CSV(Vehicle &State) {
@@ -278,6 +301,15 @@ void write2CSV(Vehicle &State) {
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");
}
@@ -325,4 +357,42 @@ void teensyAbort() {
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;
}
}