mirror of
https://gitlab.com/lander-team/lander-cpp.git
synced 2025-06-16 07:06:51 +00:00
151 lines
2.9 KiB
C++
151 lines
2.9 KiB
C++
#define M_PI 3.14159265359
|
|
|
|
#if defined(NATIVE) || defined(_WIN32)
|
|
#include <cmath>
|
|
#include <fstream>
|
|
#include <iostream>
|
|
#include <stdexcept> // std::runtime_error
|
|
#include <stdio.h>
|
|
#include <string>
|
|
#include <vector>
|
|
#elif defined(TEENSY)
|
|
#include <Arduino.h>
|
|
|
|
int BUILTIN_LED = 13;
|
|
unsigned long last, initTime;
|
|
#endif
|
|
|
|
#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);
|
|
|
|
// 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(1000);
|
|
init_Vehicle(State);
|
|
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);
|
|
|
|
State.stepDuration = micros() - last;
|
|
write2CSV(State);
|
|
|
|
// Set "prev" 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");
|
|
|
|
closeFile();
|
|
delay(20000);
|
|
|
|
Serial.println("SUCCESS");
|
|
Serial.println("Aborting Sim");
|
|
teensyAbort();
|
|
}
|
|
|
|
delay(20 - ((micros() - last) * 1000.0));
|
|
}
|
|
#endif
|
|
|
|
#if defined(_WIN32) || defined(NATIVE)
|
|
int main() {
|
|
|
|
setup();
|
|
|
|
do {
|
|
loop();
|
|
} while ((State.z > 0.0) || (State.thrustFiring != 2));
|
|
|
|
return 0;
|
|
}
|
|
#endif |