mirror of
https://gitlab.com/lander-team/lander-cpp.git
synced 2025-06-16 07:06:51 +00:00
fixed, gonna make some more changes to make sure we're utilizing our whole burn
This commit is contained in:
bpmcgeeney
parent
fe1a5b9bc0
commit
87fb8f8621
@ -28,47 +28,47 @@ struct Vehicle {
|
||||
double I11, I22, I33;
|
||||
double I11dot, I22dot, I33dot;
|
||||
|
||||
int maxServo;
|
||||
int maxServoRate;
|
||||
double maxServo;
|
||||
double maxServoRate;
|
||||
double xServoDegs, yServoDegs;
|
||||
double xServoDegsDot, yServoDegsDot;
|
||||
|
||||
double Kp, Ki, Kd;
|
||||
double yError, yPrevError;
|
||||
double pError, pPrevError;
|
||||
double i_yError, i_pError = 0;
|
||||
double i_yError, i_pError = 0.0;
|
||||
double d_yError, d_pError;
|
||||
|
||||
double simTime;
|
||||
int stepSize;
|
||||
double stepSize;
|
||||
|
||||
int time = 0;
|
||||
double time = 0.0;
|
||||
};
|
||||
|
||||
void init_Vehicle(Vehicle &State) {
|
||||
// PID Gains
|
||||
State.Kp = -6.8699;
|
||||
State.Ki = 0;
|
||||
State.Ki = 0.0;
|
||||
State.Kd = -0.775;
|
||||
|
||||
// Initial Velocity
|
||||
State.vx = 0; // [m/s]
|
||||
State.vy = 0; // [m/s]
|
||||
State.vz = 0; // [m/s]
|
||||
State.vx = 0.0; // [m/s]
|
||||
State.vy = 0.0; // [m/s]
|
||||
State.vz = 0.0; // [m/s]
|
||||
|
||||
// Initial YPR
|
||||
State.yaw = 45 * M_PI / 180; // [rad]
|
||||
State.pitch = 45 * M_PI / 180; // [rad]
|
||||
State.roll = 0 * M_PI / 180; // [rad]
|
||||
State.yaw = 45.0 * M_PI / 180.0; // [rad]
|
||||
State.pitch = 45.0 * M_PI / 180.0; // [rad]
|
||||
State.roll = 0.0 * M_PI / 180.0; // [rad]
|
||||
|
||||
// Initial YPRdot
|
||||
State.yawdot = 1 * M_PI / 180; // [rad/s]
|
||||
State.pitchdot = -1 * M_PI / 180; // [rad/s]
|
||||
State.rolldot = 0 * M_PI / 180; // [rad/s]
|
||||
State.yawdot = 1.0 * M_PI / 180.0; // [rad/s]
|
||||
State.pitchdot = -1.0 * M_PI / 180.0; // [rad/s]
|
||||
State.rolldot = 0.0 * M_PI / 180.0; // [rad/s]
|
||||
|
||||
// Servo Limitation
|
||||
State.maxServo = 7; // [degs]
|
||||
State.maxServoRate = 360; // [degs/sec]
|
||||
State.maxServo = 7.0; // [degs]
|
||||
State.maxServoRate = 360.0; // [degs/sec]
|
||||
|
||||
// Vehicle Properties
|
||||
State.massInitial = 1.2; // [kg]
|
||||
@ -77,7 +77,7 @@ void init_Vehicle(Vehicle &State) {
|
||||
State.momentArm = 0.145; // [m]
|
||||
|
||||
// Sim Step Size
|
||||
State.stepSize = 1; // [ms]
|
||||
State.stepSize = 1.0; // [ms]
|
||||
|
||||
// Other Properties
|
||||
State.massPropellant = 0.06; // [kg]
|
||||
|
||||
@ -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) {
|
||||
|
||||
@ -17,11 +17,11 @@ File dataFile;
|
||||
double loadCellCalibrate() {
|
||||
// place code to calibrate load cells in here
|
||||
double loadTotal;
|
||||
for (double t = 0; t == 10; ++t) {
|
||||
loadTotal += 1;
|
||||
for (double t = 0.0; t == 10.0; ++t) {
|
||||
loadTotal += 1.0;
|
||||
delay(15);
|
||||
}
|
||||
return loadTotal / 10;
|
||||
return loadTotal / 10.0;
|
||||
}
|
||||
|
||||
void initFile() {
|
||||
@ -39,7 +39,7 @@ void initFile() {
|
||||
const char *fileName;
|
||||
|
||||
if (SD.exists("simOut.csv")) {
|
||||
while (i > 0) {
|
||||
while (i > 0.0) {
|
||||
fileName = ("simOut_" + String(i) + ".csv").c_str();
|
||||
|
||||
if (!SD.exists(fileName)) {
|
||||
@ -54,7 +54,7 @@ void initFile() {
|
||||
Serial.println("Error opening output file. \n\nABORTING SIMULATION");
|
||||
teensyAbort();
|
||||
}
|
||||
i = 0;
|
||||
i = 0.0;
|
||||
|
||||
} else {
|
||||
i++;
|
||||
@ -87,23 +87,22 @@ void thrustInfo(Vehicle &State) {
|
||||
if (State.burnElapsed != 2000) {
|
||||
// determine where in the thrust curve we're at based on elapsed burn time
|
||||
// as well as current mass
|
||||
State.burnElapsed = (State.time - State.burnStart) / 1000;
|
||||
State.burnElapsed = (State.time - State.burnStart) / 1000.0;
|
||||
State.mass = State.massInitial - (State.mdot * State.burnElapsed);
|
||||
}
|
||||
|
||||
else if (abs(State.burnVelocity + State.vz) < 0.001) {
|
||||
} else if (abs(State.burnVelocity + State.vz) < 0.01) {
|
||||
// Start burn
|
||||
State.burnStart = State.time;
|
||||
State.burnElapsed = 0;
|
||||
}
|
||||
|
||||
else
|
||||
} else {
|
||||
State.burnElapsed = 2000; // arbitrary number to ensure we don't burn
|
||||
}
|
||||
//Serial.println(abs(State.burnVelocity + State.vz));
|
||||
|
||||
if ((State.burnElapsed > 0.147) && (State.burnElapsed < 0.420)) {
|
||||
State.thrustFiring = true;
|
||||
State.thrust = 65.165 * State.burnElapsed - 2.3921;
|
||||
|
||||
} else if ((State.burnElapsed > 0.419) && (State.burnElapsed < 3.383))
|
||||
State.thrust = 0.8932 * pow(State.burnElapsed, 6) -
|
||||
11.609 * pow(State.burnElapsed, 5) +
|
||||
@ -117,25 +116,25 @@ void thrustInfo(Vehicle &State) {
|
||||
|
||||
if (State.burnElapsed > 3.45) {
|
||||
State.thrustFiring = false;
|
||||
State.thrust = 0;
|
||||
State.thrust = 0.0;
|
||||
}
|
||||
}
|
||||
|
||||
void processTVC(Vehicle &State) {
|
||||
if (State.time == 0) {
|
||||
if (State.time == 0.0) {
|
||||
Serial.println("WARNING: processTVC not implemented for TEENSY");
|
||||
}
|
||||
|
||||
// Vector math to aqcuire thrust vector components
|
||||
State.Fx = State.thrust * sin(State.xServoDegs * (M_PI / 180));
|
||||
State.Fy = State.thrust * sin(State.yServoDegs * (M_PI / 180));
|
||||
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;
|
||||
State.momentZ = 0.0;
|
||||
}
|
||||
|
||||
void write2CSV(Vehicle &State) {
|
||||
@ -163,18 +162,18 @@ void write2CSV(Vehicle &State) {
|
||||
dataFile.print(State.az);
|
||||
dataFile.print(",");
|
||||
|
||||
dataFile.print(State.yaw * 180 / M_PI);
|
||||
dataFile.print(State.yaw * 180.0 / M_PI);
|
||||
dataFile.print(",");
|
||||
dataFile.print(State.pitch * 180 / M_PI);
|
||||
dataFile.print(State.pitch * 180.0 / M_PI);
|
||||
dataFile.print(",");
|
||||
dataFile.print(State.roll * 180 / M_PI);
|
||||
dataFile.print(State.roll * 180.0 / M_PI);
|
||||
dataFile.print(",");
|
||||
|
||||
dataFile.print(State.yawdot * 180 / M_PI);
|
||||
dataFile.print(State.yawdot * 180.0 / M_PI);
|
||||
dataFile.print(",");
|
||||
dataFile.print(State.pitchdot * 180 / M_PI);
|
||||
dataFile.print(State.pitchdot * 180.0 / M_PI);
|
||||
dataFile.print(",");
|
||||
dataFile.print(State.rolldot * 180 / M_PI);
|
||||
dataFile.print(State.rolldot * 180.0 / M_PI);
|
||||
dataFile.print(",");
|
||||
|
||||
dataFile.print(State.xServoDegs);
|
||||
@ -195,9 +194,9 @@ void write2CSV(Vehicle &State) {
|
||||
}
|
||||
|
||||
void printSimResults(Vehicle &State) {
|
||||
State.yaw = State.yaw * 180 / M_PI;
|
||||
State.pitch = State.pitch * 180 / M_PI;
|
||||
State.roll = State.roll * 180 / M_PI;
|
||||
State.yaw = State.yaw * 180.0 / M_PI;
|
||||
State.pitch = State.pitch * 180.0 / M_PI;
|
||||
State.roll = State.roll * 180.0 / M_PI;
|
||||
|
||||
double landing_angle =
|
||||
pow(State.yaw * State.yaw + State.pitch * State.pitch, .5);
|
||||
|
||||
@ -43,6 +43,7 @@ void setup() {
|
||||
|
||||
// Determine when to burn
|
||||
burnStartTimeCalc(State);
|
||||
Serial.println(State.burnVelocity);
|
||||
Serial.println("Starting Height Calculated");
|
||||
delay(1000);
|
||||
loadCellCalibrate();
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user