Anson/Lander-Embedded
1
0
Fork 0
mirror of https://gitlab.com/lander-team/lander-cpp.git synced 2025-06-16 15:17:23 +00:00
Code Releases Activity

added abort conditions for load cells and tvc

Browse Source
This commit is contained in:
bpmcgeeney 2021-11-29 10:42:48 -07:00
parent 7993aa752d
commit a47cef4bc9
1 changed files with 26 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
include/teensy.h
View File

@ -47,6 +47,11 @@ File dataFile;
void testGimbal(PWMServo &yaw, PWMServo &pitch) {
int servoTest = 90;
if ((!yaw.attached()) || (!pitch.attached())) {
Serial.println("Servo pin assignments are not valid!");
teensyAbort();
}
yaw.write(yaw_conv(servoTest));
pitch.write(pitch_conv(servoTest));
@ -87,7 +92,7 @@ void testGimbal(PWMServo &yaw, PWMServo &pitch) {
}
delay(30);
// BRING IN YAW AND PITCH CONVERSION FROM TVC TEST
yaw.write(yaw_conv(90));
pitch.write(pitch_conv(90));
}
@ -127,7 +132,23 @@ void initLoadCells(Vehicle &State) {
attachInterrupt(digitalPinToInterrupt(pin_lc2), read_lc2, LOW);
attachInterrupt(digitalPinToInterrupt(pin_lc3), read_lc3, LOW);
Serial.println("Load Cells Initialized");
Serial.println("Checking tare");
double lc0Avg, lc1Avg, lc2Avg, lc3Avg = 0.0;
double loopSize = 50.0;
for (int i = 0; i < loopSize; i++) {
lc0Avg += (State.lc0 / loopSize);
lc1Avg += (State.lc1 / loopSize);
lc2Avg += (State.lc2 / loopSize);
lc3Avg += (State.lc3 / loopSize);
delay(50);
}
if (((lc0Avg > 1000) || (lc1Avg > 1000)) ||
((lc2Avg > 1000) || (lc3Avg > 1000))) {
Serial.println("Load cell tare insufficient!");
teensyAbort();
}
// Initializes State variables
State.lc0 = lc0.get_value();
@ -135,6 +156,8 @@ void initLoadCells(Vehicle &State) {
State.lc2 = lc2.get_value();
State.lc3 = lc3.get_value();
Serial.println("Load Cells Initialized");
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;
@ -388,7 +411,6 @@ void read_lc3() {
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;
@ -397,6 +419,7 @@ double loadCellFilter(double current, double previous) {
}
}
// Desired TVC angle to servo input conversion
float yaw_conv(float thetad) {
float h = .2875;
float H = 1.6;