diff --git a/include/LoadCells.h b/include/LoadCells.h deleted file mode 100644 index 7aefd5d..0000000 --- a/include/LoadCells.h +++ /dev/null @@ -1,37 +0,0 @@ - -#include - -struct LoadCells { - HX711 loadcell_0; - HX711 loadcell_1; - HX711 loadcell_2; - HX711 loadcell_3; - - double lc0Value; - double lc1Value; - double lc2Value; - double lc3Value; - - double lc0Calibration; - double lc1Calibration; - double lc2Calibration; - double lc3Calibration; -}; - -double loadCellCalibrate(HX711 loadCell) { - // place code to calibrate load cells in here - double loadTotal = 0.0; - for (int t = 0; t == 10; ++t) { - loadTotal += loadCell.read(); - delay(15); - } - return loadTotal / 10.0; -} - -void init_LoadCells(LoadCells &loadCells) { - - loadCells.lc0Calibration = loadCellCalibrate(loadCells.loadcell_0); - loadCells.lc1Calibration = loadCellCalibrate(loadCells.loadcell_1); - loadCells.lc2Calibration = loadCellCalibrate(loadCells.loadcell_2); - loadCells.lc3Calibration = loadCellCalibrate(loadCells.loadcell_3); -} \ No newline at end of file diff --git a/include/Vehicle.h b/include/Vehicle.h index a5de21e..ed47313 100644 --- a/include/Vehicle.h +++ b/include/Vehicle.h @@ -22,7 +22,7 @@ struct Vehicle { double thrust, burnElapsed, burnStart; int thrustFiring = 0; // 0: Pre-burn, 1: Mid-burn, 2: Post-burn - double PIDx, PIDy, Fx, Fy, Fz; + double PIDx, PIDy, Fx, Fy, Fz, F; double momentX, momentY, momentZ; double I11, I22, I33; @@ -44,6 +44,8 @@ struct Vehicle { double stepDuration; double time = 0.0; + + double lc0, lc1, lc2, lc3; }; void init_Vehicle(Vehicle &State) { @@ -78,7 +80,7 @@ void init_Vehicle(Vehicle &State) { State.momentArm = 0.145; // [m] // Sim Step Size - State.stepSize = 1.0; // [ms] + State.stepSize = 5.0; // [ms] // Other Properties State.massPropellant = 0.06; // [kg] diff --git a/include/native.h b/include/native.h index d12f87a..1cd18fa 100644 --- a/include/native.h +++ b/include/native.h @@ -117,10 +117,10 @@ void printSimResults(Vehicle &State) { std::cout << "Final Angles: [" << State.yaw << ", " << State.pitch << "]" << std::endl; - if (landing_velocity < 0.5) { - std::cout << "Landing Velocity < 0.5 m/s | PASS | "; + if (landing_velocity < 1.0) { + std::cout << "Landing Velocity < 1.0 m/s | PASS | "; } else { - std::cout << "Landing Velocity < 0.5 m/s | FAIL | "; + std::cout << "Landing Velocity < 1.0 m/s | FAIL | "; } std::cout << "Final Velocity: [" << State.vx << ", " << State.vy << ", " << State.vz << "]" << std::endl; diff --git a/include/teensy.h b/include/teensy.h index b2056b9..0ad10f4 100644 --- a/include/teensy.h +++ b/include/teensy.h @@ -1,12 +1,14 @@ #include "Vehicle.h" #include +#include #include #include +void testGimbal(class PWMServo &, class PWMServo &); double loadCellCalibrate(); void initFile(); void thrustInfo(struct Vehicle &); -void processTVC(struct Vehicle &); +void processTVC(struct Vehicle &, class PWMServo &, class PWMServo &); void write2CSV(struct Vehicle &); void printSimResults(struct Vehicle &); void teensyAbort(); @@ -14,14 +16,51 @@ void teensyAbort(); const int chipSelect = BUILTIN_SDCARD; File dataFile; -double loadCellCalibrate() { - // place code to calibrate load cells in here - double loadTotal = 0.0; - for (double t = 0.0; t == 10.0; ++t) { - loadTotal += 1.0; - delay(15); +void testGimbal(PWMServo &servo1, PWMServo &servo2) { + int servoTest = 0; + + servo1.write(servoTest); + servo2.write(servoTest); + + // Servo 1 Test + for (servoTest = 0; servoTest < 7; servoTest += 1) { + servo1.write(servoTest); + delay(30); } - return loadTotal / 10.0; + for (servoTest = 7; servoTest >= 1; servoTest -= 1) { + servo1.write(servoTest); + delay(30); + } + + delay(1000); + + // Servo 2 Test + for (servoTest = 0; servoTest < 7; servoTest += 1) { + servo2.write(servoTest); + delay(30); + } + for (servoTest = 7; servoTest >= 1; servoTest -= 1) { + servo2.write(servoTest); + delay(30); + } + + delay(1000); + + // Servo 1 & 2 Test + for (servoTest = 0; servoTest < 7; servoTest += 1) { + servo1.write(servoTest); + servo2.write(servoTest); + delay(30); + } + for (servoTest = 7; servoTest >= 1; servoTest -= 1) { + servo1.write(servoTest); + servo2.write(servoTest); + delay(30); + } + + delay(30); + servo1.write(0); + servo2.write(0); } void initFile() { @@ -75,7 +114,7 @@ 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"); + "Servo1,Servo2,thrustFiring,thrust,simResponse,lc0,lc1,lc2,lc3"); } void thrustInfo(Vehicle &State) { @@ -103,16 +142,21 @@ void thrustInfo(Vehicle &State) { } } -void processTVC(Vehicle &State, LoadCells &loadCells) { +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 = loadCells.lc1Value + loadCells.lc2Value; - State.Fy = loadCells.lc0Value + loadCells.lc3Value; + 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)) + (State.mass * g); @@ -120,6 +164,11 @@ void processTVC(Vehicle &State, LoadCells &loadCells) { State.momentX = State.Fx * State.momentArm; State.momentY = State.Fy * State.momentArm; State.momentZ = 0.0; + + State.F = sqrt(pow(State.Fx, 2) + pow(State.Fy, 2) + pow(State.Fz, 2)); + + servo1.write(State.xServoDegs); + servo2.write(State.yServoDegs); } void write2CSV(Vehicle &State) { @@ -172,8 +221,17 @@ void write2CSV(Vehicle &State) { dataFile.print(","); dataFile.print(String(State.stepDuration, 5)); - dataFile.print("\n"); + 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("\n"); } void printSimResults(Vehicle &State) { @@ -195,10 +253,10 @@ void printSimResults(Vehicle &State) { Serial.print("Final Angles: [" + String(State.yaw) + ", " + String(State.pitch) + "]\n"); - if (landing_velocity < 0.5) { - Serial.print("Landing Velocity < 0.5 m/s | PASS | "); + if (landing_velocity < 1.0) { + Serial.print("Landing Velocity < 1.0 m/s | PASS | "); } else { - Serial.print("Landing Velocity < 0.5 m/s | FAIL | "); + Serial.print("Landing Velocity < 1.0 m/s | FAIL | "); } Serial.print("Final Velocity: [" + String(State.vx) + ", " + String(State.vy) + ", " + String(State.vz) + "]\n"); diff --git a/src/main.cpp b/src/main.cpp index 4382951..dbc46fb 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,6 +1,4 @@ -#define M_PI 3.14159265359 - -#if defined(NATIVE) || defined(_WIN32) +#if defined(_WIN32) || defined(NATIVE) #include #include #include @@ -10,142 +8,190 @@ #include #elif defined(TEENSY) #include - -int BUILTIN_LED = 13; -unsigned long last, initTime; #endif +#define M_PI 3.14159265359 + #include "Vehicle.h" #include "sim.h" -#if defined(NATIVE) || defined(_WIN32) -#include "native.h" -outVector stateVector; -#elif defined(TEENSY) - -#include "LoadCells.h" -LoadCells loadCells; - -#include "teensy.h" -const int lc_clock = 23; - -const int lc_data_0 = 0; -const int lc_data_1 = 1; -const int lc_data_2 = 2; -const int lc_data_3 = 3; - -#endif - Vehicle State; Vehicle PrevState; -#if defined(NATIVE) || defined(_WIN32) -void setup() { - init_Vehicle(State); +#if defined(TEENSY) + +int BUILTIN_LED = 13; +unsigned long last, initTime; + +#include "teensy.h" +#include +#include + +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; + +void init_loadCells(); +void read_lc0(); +void read_lc1(); +void read_lc2(); +void read_lc3(); + +HX711 lc0; +HX711 lc1; +HX711 lc2; +HX711 lc3; + +PWMServo servo1; +PWMServo servo2; + +const int pin_servo1 = 33; +const int pin_servo2 = 29; + +int servoPos = 0; // variable to store the servo position; - // Determine when to burn - burnStartTimeCalc(State); -} -#elif defined(TEENSY) void setup() { Serial.begin(9600); - // 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(lc_data_0, INPUT); - pinMode(lc_data_1, INPUT); - pinMode(lc_data_2, INPUT); - pinMode(lc_data_3, INPUT); + delay(5000); + Serial.println("Simulation Countdown:"); + for (int i = 0; i < 10; i++) { + Serial.println(10 - i); + delay(1000); + } - delay(1000); + init_loadCells(); 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); + testGimbal(servo1, servo2); + Serial.println("Servos Tested"); + + delay(3000); Serial.println("Simulated Vehicle Initalized"); - delay(1000); // Determine when to burn burnStartTimeCalc(State); Serial.println("Starting Height Calculated"); - delay(1000); - init_LoadCells(loadCells); - - Serial.println("Load Cells Calibrated"); - delay(1000); initFile(); - delay(1000); initTime = micros(); } -#endif -#if defined(NATIVE) || defined(_WIN32) void loop() { - vehicleDynamics(State, PrevState); - thrustInfo(State); - pidController(State, PrevState); - TVC(State, PrevState); - processTVC(State); - state2vec(State, PrevState, stateVector); - - // Set "prev" values for next timestep - PrevState = State; - State.time += State.stepSize; - - if ((State.z < 0.0) && (State.thrustFiring == 2)) { - write2CSV(stateVector, State); - printSimResults(State); - init_Vehicle(State); - } -} -#elif defined(TEENSY) -void loop() { - last = micros(); + vehicleDynamics(State, PrevState); thrustInfo(State); pidController(State, PrevState); TVC(State, PrevState); - - processTVC(State, loadCells); + processTVC(State, servo1, servo2); State.stepDuration = micros() - last; write2CSV(State); - // Set "prev" values for next timestep + // Set previous values for next timestep PrevState = State; - - // state2vec(State, PrevState, stateVector); - - State.time += State.stepSize; if ((State.z < 0.0) && (State.thrustFiring == 2)) { printSimResults(State); - Serial.println("Run duration:" + String(micros() - initTime) + " us"); + Serial.println("Run duration:" + String((micros() - initTime) / 1000000.0) + + " seconds"); closeFile(); - delay(20000); + delay(3000); Serial.println("SUCCESS"); - Serial.println("Aborting Sim"); + Serial.println("Exiting Sim"); teensyAbort(); } - delay(20 - ((micros() - last) * 1000.0)); + delay(State.stepSize - ((micros() - last) / 1000.0)); } + +void init_loadCells() { + // 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); + + // 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"); + + Serial.print("Calibrating"); + lc0.tare(); + Serial.print("."); + lc1.tare(); + Serial.print("."); + lc2.tare(); + Serial.print("."); + lc3.tare(); + Serial.println("."); + Serial.println("Load Cells Calibrated"); +} + +// ISRs to print data to serial monitor +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) + +#include "native.h" +outVector stateVector; + int main() { - setup(); + init_Vehicle(State); + + // Determine when to burn + burnStartTimeCalc(State); do { - loop(); + vehicleDynamics(State, PrevState); + thrustInfo(State); + pidController(State, PrevState); + TVC(State, PrevState); + processTVC(State); + state2vec(State, PrevState, stateVector); + + // Set "prev" values for next timestep + PrevState = State; + State.time += State.stepSize; + + if ((State.z < 0.0) && (State.thrustFiring == 2)) { + write2CSV(stateVector, State); + printSimResults(State); + init_Vehicle(State); + } } while ((State.z > 0.0) || (State.thrustFiring != 2)); return 0; } -#endif \ No newline at end of file +#endif