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

corrected thrust vector calcs

Browse Source
This commit is contained in:
bpmcgeeney 2021-11-22 13:13:21 -07:00
parent 7a523bab6d
commit 121be9452e
1 changed files with 8 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
include/teensy.h
View File

@ -209,15 +209,16 @@ void thrustInfo(Vehicle &State) {
double r = 3.0;
double R = 5.0;
// Vector math to aqcuire thrust vector components
// PLACEHOLDER PLACEHOLDERPLACEHOLDER PLACEHOLDERPLACEHOLDER
// PLACEHOLDERPLACEHOLDER PLACEHOLDERPLACEHOLDER PLACEHOLDER
State.thrust = State.lc0_processed + State.lc1_processed +
State.lc2_processed + State.lc3_processed;
State.Fz = State.lc0_processed + State.lc1_processed + State.lc2_processed +
State.lc3_processed;
State.Fx = (State.lc1_processed - State.lc2_processed) * r / R;
State.Fy = (State.lc0_processed - State.lc3_processed) * r / R;
State.Fz =
sqrt(pow(State.thrust, 2) - pow(State.Fx, 2) - pow(State.Fy, 2)) +
State.thrust =
sqrt(pow(State.Fz, 2) + pow(State.Fx, 2) + pow(State.Fy, 2)) +
(state.mass * g);
} else {
@ -226,17 +227,18 @@ void thrustInfo(Vehicle &State) {
}
void processTVC(Vehicle &State, PWMServo &servo1, PWMServo &servo2) {
/*
State.Fx = State.thrust * sin(State.xServoDegs * (M_PI / 180.0));
State.Fy = State.thrust * sin(State.yServoDegs * (M_PI / 180.0));
State.Fz = sqrt(pow(State.thrust, 2) - pow(State.Fx, 2) - pow(State.Fy, 2)) +
(State.mass * g);
*/
// Calculate moment created by Fx and Fy
State.momentX = State.Fx * State.momentArm;
State.momentY = State.Fy * State.momentArm;
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(90 + State.xServoDegs);
servo2.write(90 + State.yServoDegs);