mirror of
https://gitlab.com/lander-team/lander-cpp.git
synced 2025-06-16 07:06:51 +00:00
code compiles and runs on teensy
This commit is contained in:
parent
7db2ab59f6
commit
ed816f223a
@ -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);
|
||||
|
||||
@ -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
|
||||
51
src/main.cpp
51
src/main.cpp
@ -1,5 +1,10 @@
|
||||
#define M_PI 3.14159265359
|
||||
|
||||
#ifdef TEENSY
|
||||
#endif
|
||||
#include <Arduino.h>
|
||||
unsigned long tic, toc = millis();
|
||||
|
||||
#include <cmath>
|
||||
#include <fstream>
|
||||
#include <iostream>
|
||||
@ -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);
|
||||
}
|
||||
Loading…
x
Reference in New Issue
Block a user