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

Merge branch 'main' of gitlab.com:lander-team/lander-cpp

Browse Source
This commit is contained in:
Anson 2021-10-15 14:31:46 -07:00
commit a7de8b71a3
4 changed files with 78 additions and 56 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
include/Vehicle.h
View File

@ -29,7 +29,9 @@ struct Vehicle {
double I11dot, I22dot, I33dot; double I11dot, I22dot, I33dot;
int maxServo; int maxServo;
int maxServoRate;
double xServoDegs, yServoDegs; double xServoDegs, yServoDegs;
double xServoDegsDot, yServoDegsDot;
double Kp, Ki, Kd; double Kp, Ki, Kd;
double yError, yPrevError; double yError, yPrevError;

2
include/outVector.h
View File

@ -33,6 +33,8 @@ struct outVector {
std::vector<double> PIDx = std::vector<double>(length, 0.0); std::vector<double> PIDx = std::vector<double>(length, 0.0);
std::vector<double> PIDy = std::vector<double>(length, 0.0); std::vector<double> PIDy = std::vector<double>(length, 0.0);
std::vector<double> thrust = std::vector<double>(length, 0.0);
}; };
#endif #endif

120
include/sim.h
View File

@ -6,15 +6,16 @@
void burnStartTimeCalc(struct Vehicle &); void burnStartTimeCalc(struct Vehicle &);
void thrustSelection(struct Vehicle &, int t); void thrustSelection(struct Vehicle &, int t);
void pidController(struct Vehicle &, struct Vehicle &); void pidController(struct Vehicle &, struct Vehicle &);
void TVC(struct Vehicle &); void TVC(struct Vehicle &, struct Vehicle &);
void vehicleDynamics(struct Vehicle &, struct Vehicle &, int t); void vehicleDynamics(struct Vehicle &, struct Vehicle &, int t);
void state2vec(struct Vehicle &, struct Vehicle &, struct outVector &, int t); void state2vec(struct Vehicle &, struct Vehicle &, struct outVector &, int t);
void write2CSV(struct outVector &, struct Vehicle &); void write2CSV(struct outVector &, struct Vehicle &);
double derivative(double x2, double x1, double dt); double derivative(double current, double previous, double step);
double integral(double x2, double x1, double dt); double integral(double currentChange, double prevValue, double dt);
double limit(double value, double upr, double lwr);
// Any parameters that are constants should be declared here instead of buried // Any parameters that are constants should be declared here instead of
// in code // buried in code
double const dt = 0.01; double const dt = 0.01;
double const g = -9.81; double const g = -9.81;
@ -32,7 +33,7 @@ bool sim(struct Vehicle &State, struct Vehicle &PrevState) {
vehicleDynamics(State, PrevState, t); vehicleDynamics(State, PrevState, t);
thrustSelection(State, t); thrustSelection(State, t);
pidController(State, PrevState); pidController(State, PrevState);
TVC(State); TVC(State, PrevState);
state2vec(State, PrevState, stateVector, t); state2vec(State, PrevState, stateVector, t);
t += State.stepSize; t += State.stepSize;
@ -246,20 +247,12 @@ void pidController(Vehicle &State, struct Vehicle &PrevState) {
State.PIDy = 0; State.PIDy = 0;
} }
// PID Force limiter X // PID Force Limiter
if (State.PIDx > State.thrust) State.PIDx = limit(State.PIDx, State.thrust, -State.thrust);
State.PIDx = State.thrust; State.PIDy = limit(State.PIDy, State.thrust, -State.thrust);
else if (State.PIDx < -1 * State.thrust)
State.PIDx = -1 * State.thrust;
// PID Force limiter Y
if (State.PIDy > State.thrust)
State.PIDy = State.thrust;
else if (State.PIDy < -1 * State.thrust)
State.PIDy = -1 * State.thrust;
} }
void TVC(Vehicle &State) { void TVC(Vehicle &State, Vehicle &PrevState) {
if (State.thrust < 0.1) { if (State.thrust < 0.1) {
// Define forces and moments for t = 0 // Define forces and moments for t = 0
State.Fx = 0; State.Fx = 0;
@ -273,21 +266,29 @@ void TVC(Vehicle &State) {
} else { } else {
// Convert servo position to degrees for comparison to max allowable // Convert servo position to degrees for comparison to max allowable
State.xServoDegs = (180 / M_PI) * asin(State.PIDx / State.thrust); State.xServoDegs = (180 / M_PI) * asin(State.PIDx / State.thrust);
// Servo position limiter
if (State.xServoDegs > State.maxServo)
State.xServoDegs = State.maxServo;
else if (State.xServoDegs < -1 * State.maxServo)
State.xServoDegs = -1 * State.maxServo;
// Convert servo position to degrees for comparison to max allowable
State.yServoDegs = (180 / M_PI) * asin(State.PIDy / State.thrust); State.yServoDegs = (180 / M_PI) * asin(State.PIDy / State.thrust);
// Servo position limiter // Limit Servo Position
if (State.yServoDegs > State.maxServo) State.xServoDegs = limit(State.xServoDegs, State.maxServo, -State.maxServo);
State.yServoDegs = State.maxServo; State.yServoDegs = limit(State.yServoDegs, State.maxServo, -State.maxServo);
else if (State.yServoDegs < -1 * State.maxServo)
State.yServoDegs = -1 * State.maxServo; // Servo Rate
State.xServoDegsDot =
derivative(State.xServoDegs, PrevState.xServoDegs, State.stepSize);
State.yServoDegsDot =
derivative(State.yServoDegs, PrevState.yServoDegs, State.stepSize);
// Limit Servo Rate
State.xServoDegsDot =
limit(State.xServoDegsDot, State.maxServoRate, -State.maxServoRate);
State.yServoDegsDot =
limit(State.yServoDegsDot, State.maxServoRate, -State.maxServoRate);
// Back to Degrees
State.xServoDegs =
integral(State.xServoDegsDot, PrevState.xServoDegs, State.stepSize);
State.yServoDegs =
integral(State.yServoDegsDot, PrevState.yServoDegs, State.stepSize);
// Vector math to aqcuire thrust vector components // Vector math to aqcuire thrust vector components
State.Fx = State.thrust * sin(State.xServoDegs * (M_PI / 180)); State.Fx = State.thrust * sin(State.xServoDegs * (M_PI / 180));
@ -333,6 +334,8 @@ void state2vec(Vehicle &State, Vehicle &PrevState, outVector &stateVector,
stateVector.PIDx[t] = State.PIDx; stateVector.PIDx[t] = State.PIDx;
stateVector.PIDy[t] = State.PIDy; stateVector.PIDy[t] = State.PIDy;
stateVector.thrust[t] = State.thrust;
// Set "prev" values for next timestep // Set "prev" values for next timestep
PrevState = State; PrevState = State;
} }
@ -353,40 +356,42 @@ void write2CSV(outVector &stateVector, Vehicle &State) {
// debugging // debugging
outfile << "t, x, y, z, vx, vy, vz, ax, ay, az, yaw, pitch, roll, yawdot, " outfile << "t, x, y, z, vx, vy, vz, ax, ay, az, yaw, pitch, roll, yawdot, "
"pitchdot, rolldot, Servo1, Servo2, thrustFiring, PIDx, PIDy, " "pitchdot, rolldot, Servo1, Servo2, thrustFiring, PIDx, PIDy, "
"thrust, deriv" "thrust"
<< std::endl; << std::endl;
// writing to output file // writing to output file
for (int t = 0; t < State.simTime; t += State.stepSize) { for (int t = 0; t < State.simTime; t += State.stepSize) {
outfile << t << ", "; outfile << t << ", ";
outfile << stateVector.x[t] << ", "; outfile << stateVector.x[t] << ",";
outfile << stateVector.y[t] << ", "; outfile << stateVector.y[t] << ",";
outfile << stateVector.z[t] << ", "; outfile << stateVector.z[t] << ",";
outfile << stateVector.vx[t] << ", "; outfile << stateVector.vx[t] << ",";
outfile << stateVector.vy[t] << ", "; outfile << stateVector.vy[t] << ",";
outfile << stateVector.vz[t] << ", "; outfile << stateVector.vz[t] << ",";
outfile << stateVector.ax[t] << ", "; outfile << stateVector.ax[t] << ",";
outfile << stateVector.ay[t] << ", "; outfile << stateVector.ay[t] << ",";
outfile << stateVector.az[t] << ", "; outfile << stateVector.az[t] << ",";
outfile << stateVector.yaw[t] * 180 / M_PI << ", "; outfile << stateVector.yaw[t] * 180 / M_PI << ",";
outfile << stateVector.pitch[t] * 180 / M_PI << ", "; outfile << stateVector.pitch[t] * 180 / M_PI << ",";
outfile << stateVector.roll[t] * 180 / M_PI << ", "; outfile << stateVector.roll[t] * 180 / M_PI << ",";
outfile << stateVector.yawdot[t] * 180 / M_PI << ", "; outfile << stateVector.yawdot[t] * 180 / M_PI << ",";
outfile << stateVector.pitchdot[t] * 180 / M_PI << ", "; outfile << stateVector.pitchdot[t] * 180 / M_PI << ",";
outfile << stateVector.rolldot[t] * 180 / M_PI << ", "; outfile << stateVector.rolldot[t] * 180 / M_PI << ",";
outfile << stateVector.servo1[t] << ", "; outfile << stateVector.servo1[t] << ",";
outfile << stateVector.servo2[t] << ", "; outfile << stateVector.servo2[t] << ",";
outfile << stateVector.thrustFiring[t] << ", "; outfile << stateVector.thrustFiring[t] << ",";
outfile << stateVector.PIDx[t] << ", "; outfile << stateVector.PIDx[t] << ",";
outfile << stateVector.PIDy[t] << std::endl; outfile << stateVector.PIDy[t] << ",";
outfile << stateVector.thrust[t] << std::endl;
} }
outfile.close(); outfile.close();
@ -401,3 +406,14 @@ double derivative(double current, double previous, double step) {
double integral(double currentChange, double prevValue, double dt) { double integral(double currentChange, double prevValue, double dt) {
return (currentChange * dt / 1000) + prevValue; return (currentChange * dt / 1000) + prevValue;
} }
double limit(double value, double upr, double lwr) {
if (value > upr)
value = upr;
else if (value < lwr)
value = lwr;
else
value = value;
return value;
}

10
src/main.cpp
View File

@ -28,9 +28,10 @@ int main() {
State.vz = 0; // [m/s] State.vz = 0; // [m/s]
// Initial YPR // Initial YPR
State.yaw = 75 * M_PI / 180; // [rad]
State.pitch = 35 * M_PI / 180; // [rad] State.yaw = 75 * M_PI / 180; // [rad]
State.roll = 0 * M_PI / 180; // [rad] State.pitch = 30 * M_PI / 180; // [rad]
State.roll = 0 * M_PI / 180; // [rad]
// Initial YPRdot // Initial YPRdot
State.yawdot = 1 * M_PI / 180; // [rad/s] State.yawdot = 1 * M_PI / 180; // [rad/s]
@ -38,7 +39,8 @@ int main() {
State.rolldot = 0 * M_PI / 180; // [rad/s] State.rolldot = 0 * M_PI / 180; // [rad/s]
// Servo Limitation // Servo Limitation
State.maxServo = 15; // [degs] State.maxServo = 7; // [degs]
State.maxServoRate = 360; // [degs/sec]
// Vehicle Properties // Vehicle Properties
State.massInitial = 1.2; // [kg] State.massInitial = 1.2; // [kg]