mirror of
https://gitlab.com/lander-team/lander-cpp.git
synced 2025-06-16 07:06:51 +00:00
organized main
This commit is contained in:
parent
812f62f22a
commit
0f3e267af8
184
src/main.cpp
184
src/main.cpp
@ -1,6 +1,4 @@
|
||||
#define M_PI 3.14159265359
|
||||
|
||||
#if defined(NATIVE) || defined(_WIN32)
|
||||
#if defined(_WIN32) || defined(NATIVE)
|
||||
#include <cmath>
|
||||
#include <fstream>
|
||||
#include <iostream>
|
||||
@ -10,28 +8,31 @@
|
||||
#include <vector>
|
||||
#elif defined(TEENSY)
|
||||
#include <Arduino.h>
|
||||
|
||||
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)
|
||||
Vehicle State;
|
||||
Vehicle PrevState;
|
||||
|
||||
#if defined(TEENSY)
|
||||
|
||||
int BUILTIN_LED = 13;
|
||||
unsigned long last, initTime;
|
||||
|
||||
#include "teensy.h"
|
||||
#include <HX711.h>
|
||||
const int lc_clock = 23;
|
||||
|
||||
const int lc_data_0 = 14;
|
||||
const int lc_data_1 = 15;
|
||||
const int lc_data_2 = 19;
|
||||
const int lc_data_3 = 20;
|
||||
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();
|
||||
@ -42,96 +43,30 @@ HX711 lc1;
|
||||
HX711 lc2;
|
||||
HX711 lc3;
|
||||
|
||||
#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);
|
||||
lc0.begin(lc_data_0, lc_clock);
|
||||
lc1.begin(lc_data_1, lc_clock);
|
||||
lc2.begin(lc_data_2, lc_clock);
|
||||
lc3.begin(lc_data_3, lc_clock);
|
||||
delay(5000);
|
||||
Serial.println("Simulation Countdown:") for (int i = 0; i < 15; i++) {
|
||||
Serial.println(15 - i) delay(1000);
|
||||
}
|
||||
|
||||
// Attach ISRs to load cell data pins to read data when available
|
||||
attachInterrupt(digitalPinToInterrupt(lc_data_0), read_lc0, LOW);
|
||||
attachInterrupt(digitalPinToInterrupt(lc_data_1), read_lc1, LOW);
|
||||
attachInterrupt(digitalPinToInterrupt(lc_data_2), read_lc2, LOW);
|
||||
attachInterrupt(digitalPinToInterrupt(lc_data_3), 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");
|
||||
|
||||
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();
|
||||
|
||||
Serial.println("Simulated Vehicle Initalized");
|
||||
delay(1000);
|
||||
|
||||
// Determine when to burn
|
||||
burnStartTimeCalc(State);
|
||||
Serial.println("Starting Height Calculated");
|
||||
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();
|
||||
@ -147,10 +82,11 @@ void loop() {
|
||||
State.stepDuration = micros() - last;
|
||||
write2CSV(State);
|
||||
|
||||
// \/ Comments are fee you can just write previous bro
|
||||
// Set "prev" values for next timestep
|
||||
PrevState = State;
|
||||
|
||||
// state2vec(State, PrevState, stateVector);
|
||||
// state2vec(State, PrevState, stateVector); <-- can we delete?
|
||||
|
||||
State.time += State.stepSize;
|
||||
|
||||
@ -167,26 +103,80 @@ void loop() {
|
||||
|
||||
delay(20 - ((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
|
||||
|
||||
// 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(); }
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user