diff --git a/include/Vehicle.h b/include/Vehicle.h index ed47313..3813c90 100644 --- a/include/Vehicle.h +++ b/include/Vehicle.h @@ -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) { diff --git a/include/teensy.h b/include/teensy.h index 4a1cb1f..b42f186 100644 --- a/include/teensy.h +++ b/include/teensy.h @@ -6,13 +6,20 @@ #include 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; + } } \ No newline at end of file diff --git a/src/main.cpp b/src/main.cpp index e8cd585..8e1a0f4 100644 --- a/src/main.cpp +++ b/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 #include -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)