Anson/Lander-Embedded
1
0
Fork 0
mirror of https://gitlab.com/lander-team/lander-cpp.git synced 2025-08-02 11:31:34 +00:00
Code Releases Activity

code compiles and runs on teensy

Browse Source
This commit is contained in:
Anson Biggs
2021-10-28 21:53:12 -07:00
parent 7db2ab59f6
commit ed816f223a
3 changed files with 104 additions and 51 deletions
Download Patch File Download Diff File Expand all files Collapse all files

99
include/sim.h
View File

@@ -1,6 +1,10 @@
#include "Vehicle.h"
#include "outVector.h"
// #ifdef DARDUINO_TEENSY41
#include "Arduino.h"
// #endif
#include <iostream>
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);