From ed816f223ab8e292fd6627a8b4d1f021ea654af0 Mon Sep 17 00:00:00 2001 From: Anson Biggs Date: Thu, 28 Oct 2021 21:53:12 -0700 Subject: [PATCH] code compiles and runs on teensy --- include/sim.h | 99 +++++++++++++++++++++++++++----------------------- platformio.ini | 5 +++ src/main.cpp | 51 +++++++++++++++++++++++--- 3 files changed, 104 insertions(+), 51 deletions(-) diff --git a/include/sim.h b/include/sim.h index 6fd1b11..a0f8bd1 100644 --- a/include/sim.h +++ b/include/sim.h @@ -1,6 +1,10 @@ #include "Vehicle.h" #include "outVector.h" +// #ifdef DARDUINO_TEENSY41 +#include "Arduino.h" +// #endif + #include void burnStartTimeCalc(struct Vehicle &); @@ -9,7 +13,7 @@ void pidController(struct Vehicle &, struct Vehicle &); void TVC(struct Vehicle &, struct Vehicle &); void vehicleDynamics(struct Vehicle &, struct Vehicle &, int t); void state2vec(struct Vehicle &, struct Vehicle &, struct outVector &, int t); -void write2CSV(struct outVector &, struct Vehicle &, int t); +// void write2CSV(struct outVector &, struct Vehicle &, int t); double derivative(double current, double previous, double step); double integral(double currentChange, double prevValue, double dt); double limit(double value, double upr, double lwr); @@ -35,12 +39,12 @@ bool sim(struct Vehicle &State, struct Vehicle &PrevState) { pidController(State, PrevState); TVC(State, PrevState); state2vec(State, PrevState, stateVector, t); - //std::cout << State.vz << "\n"; + // //std::cout << State.vz << "\n"; t += State.stepSize; } while ((State.z > 0.0)); -std::cout << t << "\n"; - write2CSV(stateVector, State, t); + std::cout << t << "\n"; + // write2CSV(stateVector, State, t); std::cout << t << "\n"; bool pass = 1; @@ -91,6 +95,8 @@ void burnStartTimeCalc(Vehicle &State) { else if ((i > 3.382) && (i < 3.46)) thrust = -195.78 * i + 675.11; + else + thrust = 0; velocity = (((thrust / mass) + g) * dt) + velocity; h = (((thrust / mass) + g) * dt) + h; @@ -341,62 +347,63 @@ void state2vec(Vehicle &State, Vehicle &PrevState, outVector &stateVector, PrevState = State; } -void write2CSV(outVector &stateVector, Vehicle &State, int t) { +// void write2CSV(outVector &stateVector, Vehicle &State, int t) { - // Deleting any previous output file - if (remove("simOut.csv") != 0) - perror("No file deletion necessary"); - else - puts("Previous output file successfully deleted"); +// // Deleting any previous output file +// if (remove("simOut.csv") != 0) +// perror("No file deletion necessary"); +// else +// puts("Previous output file successfully deleted"); - // Define and open output file "simOut.csv" - std::fstream outfile; - outfile.open("simOut.csv", std::ios::app); +// // Define and open output file "simOut.csv" +// std::fstream outfile; +// outfile.open("simOut.csv", std::ios::app); - // Output file header. These are the variables that we output - useful for - // debugging - outfile << "t,x,y,z,vx,vy,vz,ax,ay,az,yaw,pitch,roll,yawdot,pitchdot,rolldot," - "Servo1,Servo2,thrustFiring,PIDx,PIDy,thrust" - << std::endl; +// // Output file header. These are the variables that we output - useful for +// // debugging +// outfile << +// "t,x,y,z,vx,vy,vz,ax,ay,az,yaw,pitch,roll,yawdot,pitchdot,rolldot," +// "Servo1,Servo2,thrustFiring,PIDx,PIDy,thrust" +// << std::endl; - // writing to output file - for (int i = 0; i < t; i += State.stepSize) { - outfile << i << ", "; +// // writing to output file +// for (int i = 0; i < t; i += State.stepSize) { +// outfile << i << ", "; - outfile << stateVector.x[i] << ","; - outfile << stateVector.y[i] << ","; - outfile << stateVector.z[i] << ","; +// outfile << stateVector.x[i] << ","; +// outfile << stateVector.y[i] << ","; +// outfile << stateVector.z[i] << ","; - outfile << stateVector.vx[i] << ","; - outfile << stateVector.vy[i] << ","; - outfile << stateVector.vz[i] << ","; +// outfile << stateVector.vx[i] << ","; +// outfile << stateVector.vy[i] << ","; +// outfile << stateVector.vz[i] << ","; - outfile << stateVector.ax[i] << ","; - outfile << stateVector.ay[i] << ","; - outfile << stateVector.az[i] << ","; +// outfile << stateVector.ax[i] << ","; +// outfile << stateVector.ay[i] << ","; +// outfile << stateVector.az[i] << ","; - outfile << stateVector.yaw[i] * 180 / M_PI << ","; - outfile << stateVector.pitch[i] * 180 / M_PI << ","; - outfile << stateVector.roll[i] * 180 / M_PI << ","; +// outfile << stateVector.yaw[i] * 180 / M_PI << ","; +// outfile << stateVector.pitch[i] * 180 / M_PI << ","; +// outfile << stateVector.roll[i] * 180 / M_PI << ","; - outfile << stateVector.yawdot[i] * 180 / M_PI << ","; - outfile << stateVector.pitchdot[i] * 180 / M_PI << ","; - outfile << stateVector.rolldot[i] * 180 / M_PI << ","; +// outfile << stateVector.yawdot[i] * 180 / M_PI << ","; +// outfile << stateVector.pitchdot[i] * 180 / M_PI << ","; +// outfile << stateVector.rolldot[i] * 180 / M_PI << ","; - outfile << stateVector.servo1[i] << ","; - outfile << stateVector.servo2[i] << ","; +// outfile << stateVector.servo1[i] << ","; +// outfile << stateVector.servo2[i] << ","; - outfile << stateVector.thrustFiring[i] << ","; +// outfile << stateVector.thrustFiring[i] << ","; - outfile << stateVector.PIDx[i] << ","; - outfile << stateVector.PIDy[i] << ","; +// outfile << stateVector.PIDx[i] << ","; +// outfile << stateVector.PIDy[i] << ","; - outfile << stateVector.thrust[i] << std::endl; - } +// outfile << stateVector.thrust[i] << std::endl; +// } - outfile.close(); - std::cout << "simOut.csv created successfully.\n" << std::endl; -} +// outfile.close(); +// //std::cout << "simOut.csv created successfully.\n" << std::endl; +// } double derivative(double current, double previous, double step) { double dxdt = (current - previous) / (step / 1000); diff --git a/platformio.ini b/platformio.ini index a13d034..25bcff9 100644 --- a/platformio.ini +++ b/platformio.ini @@ -8,7 +8,12 @@ ; Please visit documentation for the other options and examples ; https://docs.platformio.org/page/projectconf.html +[env:WINDOWS] +platform = windows_x86 +build_flags = -std=c++11 -Wall -O1 + [env:teensy41] platform = teensy board = teensy41 framework = arduino +build_flags = -std=c++11 -Wall -O1 -TEENSY \ No newline at end of file diff --git a/src/main.cpp b/src/main.cpp index ee7d759..9feb288 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,5 +1,10 @@ #define M_PI 3.14159265359 +#ifdef TEENSY +#endif +#include +unsigned long tic, toc = millis(); + #include #include #include @@ -13,7 +18,7 @@ bool sim(struct Vehicle &); -int main() { +int run_simulation() { Vehicle State; Vehicle PrevState; @@ -28,9 +33,9 @@ int main() { State.vz = 0; // [m/s] // Initial YPR - State.yaw = 10 * M_PI / 180; // [rad] + State.yaw = 10 * M_PI / 180; // [rad] State.pitch = 5 * M_PI / 180; // [rad] - State.roll = 0 * M_PI / 180; // [rad] + State.roll = 0 * M_PI / 180; // [rad] // Initial YPRdot State.yawdot = 1 * M_PI / 180; // [rad/s] @@ -38,7 +43,7 @@ int main() { State.rolldot = 0 * M_PI / 180; // [rad/s] // Servo Limitation - State.maxServo = 7; // [degs] + State.maxServo = 7; // [degs] State.maxServoRate = 360; // [degs/sec] // Vehicle Properties @@ -64,5 +69,41 @@ int main() { std::cout << std::endl << "Simulation Complete 🚀" << std::endl; // ^^^ // 50% chance this makes Mattys linux crash - return 0; + return outcome; +} + +#ifndef LED_ON +#define LED_ON (HIGH) +#endif + +#ifndef LED_BUILTIN +#define LED_BUILTIN (13) +#endif + +#ifndef LED +#define LED (LED_BUILTIN) +#endif + +void setup() { + // initialize the digital pin as an output. + delay(10000); + pinMode(LED, OUTPUT); + Serial.begin(115200); + Serial.println("hello world!"); +} + +void loop() { + + Serial.println("blink on"); + digitalWrite(LED, LED_ON); // set the LED on + + tic = millis(); + run_simulation(); + toc = millis(); + + Serial.println("SIM RESULT:"); + Serial.println(toc - tic); + Serial.println("blink off"); + digitalWrite(LED, !LED_ON); // set the LED off + delay(1000); } \ No newline at end of file