Anson/Lander-Embedded
1
0
Fork 0
mirror of https://gitlab.com/lander-team/lander-cpp.git synced 2025-07-24 23:21:29 +00:00
Code Releases Activity

fixed, gonna make some more changes to make sure we're utilizing our whole burn

Browse Source
This commit is contained in:
bpmcgeeney
2021-11-08 14:43:52 -07:00
parent fe1a5b9bc0
commit 87fb8f8621
4 changed files with 70 additions and 71 deletions
Download Patch File Download Diff File Expand all files Collapse all files

55
include/sim.h
View File

@@ -19,7 +19,7 @@ double const g = -9.81;
void burnStartTimeCalc(Vehicle &State) {
double velocity = State.vz;
double h = 0;
double h = 0.0;
double mass, thrust;
@@ -37,39 +37,39 @@ void burnStartTimeCalc(Vehicle &State) {
else if ((i > 3.382) && (i < 3.46))
thrust = -195.78 * i + 675.11;
else
thrust = 0;
thrust = 0.0;
velocity = (((thrust / mass) + g) * dt) + velocity;
h = (((thrust / mass) + g) * dt) + h;
}
State.z = h + (pow(velocity, 2) / (2 * -g)); // starting height
State.z = 18.9;
State.z = h + (pow(velocity, 2) / (2.0 * -g)); // starting height
State.z = 19.05;
State.burnVelocity = velocity; // terminal velocity
double burnStartTime = State.burnVelocity / -g;
State.simTime = (State.burntime + burnStartTime) * 1000;
State.simTime = (State.burntime + burnStartTime) * 1000.0;
}
void vehicleDynamics(Vehicle &State, Vehicle &PrevState) {
// Moment of Inertia
State.I11 = State.mass * ((1 / 12) * pow(State.vehicleHeight, 2) +
pow(State.vehicleRadius, 2) / 4);
State.I22 = State.mass * ((1 / 12) * pow(State.vehicleHeight, 2) +
pow(State.vehicleRadius, 2) / 4);
State.I11 = State.mass * ((1 / 12.0) * pow(State.vehicleHeight, 2) +
pow(State.vehicleRadius, 2) / 4.0);
State.I22 = State.mass * ((1 / 12.0) * pow(State.vehicleHeight, 2) +
pow(State.vehicleRadius, 2) / 4.0);
State.I33 = State.mass * 0.5 * pow(State.vehicleRadius, 2);
// Idot
if (State.time < 0.1) {
State.I11dot = 0;
State.I22dot = 0;
State.I33dot = 0;
State.I11dot = 0.0;
State.I22dot = 0.0;
State.I33dot = 0.0;
State.x = 0;
State.y = 0;
State.x = 0.0;
State.y = 0.0;
State.ax = 0;
State.ay = 0;
State.ax = 0.0;
State.ay = 0.0;
State.az = State.Fz / State.massInitial;
} else {
@@ -145,7 +145,6 @@ void pidController(Vehicle &State, struct Vehicle &PrevState) {
State.d_pError = derivative(State.pError, PrevState.pError, State.stepSize);
// TVC block properly
State.PIDx = (State.Kp * State.yError + State.Ki * State.i_yError +
State.Kd * State.d_yError) /
State.momentArm;
@@ -154,8 +153,8 @@ void pidController(Vehicle &State, struct Vehicle &PrevState) {
State.momentArm;
} else {
State.PIDx = 0;
State.PIDy = 0;
State.PIDx = 0.0;
State.PIDy = 0.0;
}
// PID Force Limiter
@@ -166,18 +165,18 @@ void pidController(Vehicle &State, struct Vehicle &PrevState) {
void TVC(Vehicle &State, Vehicle &PrevState) {
if (State.thrust < 0.1) {
// Define forces and moments for t = 0
State.Fx = 0;
State.Fy = 0;
State.Fx = 0.0;
State.Fy = 0.0;
State.Fz = g * State.massInitial;
State.momentX = 0;
State.momentY = 0;
State.momentZ = 0;
State.momentX = 0.0;
State.momentY = 0.0;
State.momentZ = 0.0;
} else {
// Convert servo position to degrees for comparison to max allowable
State.xServoDegs = (180 / M_PI) * asin(State.PIDx / State.thrust);
State.yServoDegs = (180 / M_PI) * asin(State.PIDy / State.thrust);
State.xServoDegs = (180.0 / M_PI) * asin(State.PIDx / State.thrust);
State.yServoDegs = (180.0 / M_PI) * asin(State.PIDy / State.thrust);
// Limit Servo Position
State.xServoDegs = limit(State.xServoDegs, State.maxServo, -State.maxServo);
@@ -238,12 +237,12 @@ void state2vec(Vehicle &State, Vehicle &PrevState, outVector &stateVector) {
}
double derivative(double current, double previous, double step) {
double dxdt = (current - previous) / (step / 1000);
double dxdt = (current - previous) / (step / 1000.0);
return dxdt;
}
double integral(double currentChange, double prevValue, double dt) {
return (currentChange * dt / 1000) + prevValue;
return (currentChange * dt / 1000.0) + prevValue;
}
double limit(double value, double upr, double lwr) {