mirror of
https://gitlab.com/lander-team/lander-cpp.git
synced 2025-06-15 14:46:45 +00:00
Merge branch '36-move-init_loadcells-to-teensy-h' into 'main'
Resolve "Move init_loadCells() to teensy.h" Closes #36 See merge request lander-team/lander-cpp!28
This commit is contained in:
commit
7a523bab6d
@ -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
158
include/teensy.h
@ -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;
|
||||
}
|
||||
}
|
||||
36
src/main.cpp
36
src/main.cpp
@ -11,7 +11,6 @@
|
||||
#endif
|
||||
|
||||
#define M_PI 3.14159265359
|
||||
|
||||
#include "Vehicle.h"
|
||||
#include "sim.h"
|
||||
|
||||
@ -27,23 +26,14 @@ unsigned long last, initTime;
|
||||
#include <HX711.h>
|
||||
#include <PWMServo.h>
|
||||
|
||||
HX711 lc0;
|
||||
HX711 lc1;
|
||||
HX711 lc2;
|
||||
HX711 lc3;
|
||||
|
||||
void read_lc0();
|
||||
void read_lc1();
|
||||
void read_lc2();
|
||||
void read_lc3();
|
||||
|
||||
PWMServo servo1;
|
||||
PWMServo servo2;
|
||||
|
||||
const int pin_servo1 = 33;
|
||||
const int pin_servo2 = 29;
|
||||
const int pin_igniter = 7;
|
||||
|
||||
int servoPos = 0; // variable to store the servo position;
|
||||
int servoPos = 90; // variable to store the servo position;
|
||||
|
||||
void setup() {
|
||||
Serial.begin(9600);
|
||||
@ -55,12 +45,8 @@ void setup() {
|
||||
delay(1000);
|
||||
}
|
||||
|
||||
init_loadCells(lc0, lc1, lc2, lc3);
|
||||
initLoadCells(State);
|
||||
init_Vehicle(State);
|
||||
State.lc0 = lc0.get_value();
|
||||
State.lc1 = lc1.get_value();
|
||||
State.lc2 = lc2.get_value();
|
||||
State.lc3 = lc3.get_value();
|
||||
|
||||
servo1.attach(pin_servo1);
|
||||
servo2.attach(pin_servo2);
|
||||
@ -71,11 +57,14 @@ void setup() {
|
||||
Serial.println("Simulated Vehicle Initalized");
|
||||
|
||||
// Determine when to burn
|
||||
analogWrite(pin_igniter, 0);
|
||||
burnStartTimeCalc(State);
|
||||
Serial.println("Starting Height Calculated");
|
||||
|
||||
initFile();
|
||||
|
||||
// PLACE BUTTON HERE---------------------------------------------
|
||||
|
||||
initTime = micros();
|
||||
}
|
||||
|
||||
@ -88,7 +77,6 @@ void loop() {
|
||||
TVC(State, PrevState);
|
||||
processTVC(State, servo1, servo2);
|
||||
|
||||
State.stepDuration = micros() - last;
|
||||
write2CSV(State);
|
||||
|
||||
// Set previous values for next timestep
|
||||
@ -96,9 +84,10 @@ void loop() {
|
||||
State.time += State.stepSize;
|
||||
|
||||
if ((State.z < 0.0) && (State.thrustFiring == 2)) {
|
||||
printSimResults(State);
|
||||
analogWrite(pin_igniter, 0);
|
||||
Serial.println("Run duration:" + String((micros() - initTime) / 1000000.0) +
|
||||
" seconds");
|
||||
printSimResults(State);
|
||||
|
||||
closeFile();
|
||||
delay(3000);
|
||||
@ -108,15 +97,10 @@ void loop() {
|
||||
teensyAbort();
|
||||
}
|
||||
|
||||
delay(State.stepSize - ((micros() - last) / 1000.0));
|
||||
State.stepDuration = micros() - last;
|
||||
delayMicroseconds((1000.0 * State.stepSize) - State.stepDuration);
|
||||
}
|
||||
|
||||
// ISRs to print data to serial monitor - These unfortunately can't move
|
||||
void read_lc0() { State.lc0 = lc0.get_value(); }
|
||||
void read_lc1() { State.lc1 = lc1.get_value(); }
|
||||
void read_lc2() { State.lc2 = lc2.get_value(); }
|
||||
void read_lc3() { State.lc3 = lc3.get_value(); }
|
||||
|
||||
#endif
|
||||
|
||||
#if defined(_WIN32) || defined(NATIVE)
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user