Anson/Lander-Embedded
1
0
Fork 0
mirror of https://gitlab.com/lander-team/lander-cpp.git synced 2025-06-16 15:17:23 +00:00
Code Releases Activity

Integrated servos, just neefds to be slowed down now

Browse Source
This commit is contained in:
bpmcgeeney 2021-11-14 13:59:51 -07:00
parent d487d6d30a
commit 8fa9dba55e
2 changed files with 26 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
include/teensy.h
View File

@ -1,12 +1,13 @@
#include "Vehicle.h" #include "Vehicle.h"
#include <Arduino.h> #include <Arduino.h>
#include <PWMServo.h>
#include <SD.h> #include <SD.h>
#include <SPI.h> #include <SPI.h>
double loadCellCalibrate(); double loadCellCalibrate();
void initFile(); void initFile();
void thrustInfo(struct Vehicle &); void thrustInfo(struct Vehicle &);
void processTVC(struct Vehicle &); void processTVC(struct Vehicle &, class PWMServo &, class PWMServo &);
void write2CSV(struct Vehicle &); void write2CSV(struct Vehicle &);
void printSimResults(struct Vehicle &); void printSimResults(struct Vehicle &);
void teensyAbort(); void teensyAbort();
@ -93,7 +94,7 @@ void thrustInfo(Vehicle &State) {
} }
} }
void processTVC(Vehicle &State) { void processTVC(Vehicle &State, PWMServo &servo1, PWMServo &servo2) {
if (State.time == 0) { if (State.time == 0) {
Serial.println("WARNING: processTVC not implemented for TEENSY"); Serial.println("WARNING: processTVC not implemented for TEENSY");
} }
@ -117,6 +118,9 @@ void processTVC(Vehicle &State) {
State.momentZ = 0.0; State.momentZ = 0.0;
State.F = sqrt(pow(State.Fx, 2) + pow(State.Fy, 2) + pow(State.Fz, 2)); State.F = sqrt(pow(State.Fx, 2) + pow(State.Fy, 2) + pow(State.Fz, 2));
servo1.write(State.xServoDegs);
servo2.write(State.yServoDegs);
} }
void write2CSV(Vehicle &State) { void write2CSV(Vehicle &State) {

30
src/main.cpp
View File

@ -25,6 +25,8 @@ unsigned long last, initTime;
#include "teensy.h" #include "teensy.h"
#include <HX711.h> #include <HX711.h>
#include <PWMServo.h>
const int lc_clock = 23; const int lc_clock = 23;
const int pin_lc0 = 14; const int pin_lc0 = 14;
@ -43,6 +45,14 @@ HX711 lc1;
HX711 lc2; HX711 lc2;
HX711 lc3; HX711 lc3;
PWMServo servo1;
PWMServo servo2;
const int pin_servo1 = 33;
const int pin_servo2 = 29;
int servoPos = 0; // variable to store the servo position;
void setup() { void setup() {
Serial.begin(9600); Serial.begin(9600);
@ -59,6 +69,12 @@ void setup() {
State.lc1 = lc1.get_value(); State.lc1 = lc1.get_value();
State.lc2 = lc2.get_value(); State.lc2 = lc2.get_value();
State.lc3 = lc3.get_value(); State.lc3 = lc3.get_value();
servo1.attach(pin_servo1);
servo2.attach(pin_servo2);
servo1.write(0);
servo2.write(0);
Serial.println("Simulated Vehicle Initalized"); Serial.println("Simulated Vehicle Initalized");
// Determine when to burn // Determine when to burn
@ -69,27 +85,21 @@ void setup() {
initTime = micros(); initTime = micros();
} }
void loop() {
void loop() {
last = micros(); last = micros();
Serial.println(State.time);
vehicleDynamics(State, PrevState); vehicleDynamics(State, PrevState);
thrustInfo(State); thrustInfo(State);
pidController(State, PrevState); pidController(State, PrevState);
TVC(State, PrevState); TVC(State, PrevState);
processTVC(State, servo1, servo2);
processTVC(State);
delay(2);
State.stepDuration = micros() - last; State.stepDuration = micros() - last;
write2CSV(State); write2CSV(State);
// \/ Comments are fee you can just write previous bro // Set previous values for next timestep
// Set "prev" values for next timestep
PrevState = State; PrevState = State;
// state2vec(State, PrevState, stateVector); <-- can we delete?
State.time += State.stepSize; State.time += State.stepSize;
if ((State.z < 0.0) && (State.thrustFiring == 2)) { if ((State.z < 0.0) && (State.thrustFiring == 2)) {