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

Initial Pass, still lots of bugs to fix

Browse Source
This commit is contained in:
bpmcgeeney 2021-09-20 10:34:20 -07:00
parent 7080188063
commit 768bcb7964
6 changed files with 121 additions and 44 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
.vscode/settings.json vendored
View File

@ -39,7 +39,10 @@
"xstring": "cpp",
"xtr1common": "cpp",
"xutility": "cpp",
"vector": "cpp"
"vector": "cpp",
"cctype": "cpp",
"sstream": "cpp",
"string": "cpp"
},
"C_Cpp.clang_format_fallbackStyle": "LLVM",
"editor.formatOnSave": true,

6
include/Vehicle.h
View File

@ -21,7 +21,6 @@ struct Vehicle {
double burnVelocity;
double thrust, burnElapsed, burnStart;
bool thrustFiring = false;
;
double LQRx, LQRy, Fx, Fy, Fz;
double momentX, momentY, momentZ;
@ -32,6 +31,11 @@ struct Vehicle {
int maxServo;
double xServoDegs, yServoDegs;
double Kp, Ki, Kd;
double yError, yPrevError;
double pError, pPrevError;
double y_pidRefeed, p_pidRefeed, i_yError, i_pError = 0;
double simTime;
int stepSize;
};

3
include/outVector.h
View File

@ -30,6 +30,9 @@ struct outVector {
std::vector<double> servo2 = std::vector<double>(length, 0.0);
std::vector<bool> thrustFiring = std::vector<bool>(length, 0.0);
std::vector<double> LQRx = std::vector<double>(length, 0.0);
std::vector<double> LQRy = std::vector<double>(length, 0.0);
};
#endif

62
include/sim.h
View File

@ -4,6 +4,7 @@
void burnStartTimeCalc(struct Vehicle &);
void thrustSelection(struct Vehicle &, int t);
void lqrCalc(struct Vehicle &);
void pidController(struct Vehicle &);
void TVC(struct Vehicle &);
void vehicleDynamics(struct Vehicle &, struct Vehicle &, int t);
void state2vec(struct Vehicle &, struct outVector &, int t);
@ -28,7 +29,8 @@ bool sim(struct Vehicle &State, struct Vehicle &PrevState) {
// Start Sim
do {
thrustSelection(State, t);
lqrCalc(State);
// lqrCalc(State);
pidController(State);
TVC(State);
vehicleDynamics(State, PrevState, t);
state2vec(State, stateVector, t);
@ -177,6 +179,54 @@ void lqrCalc(Vehicle &State) {
State.LQRy = -1 * State.thrust;
}
void pidController(Vehicle &State) {
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.I33 = State.mass * 0.5 * pow(State.vehicleRadius, 2);
if (State.thrust > 1) {
State.yError = State.yaw; // - State.y_pidRefeed;
State.pError = State.pitch; // - State.p_pidRefeed;
State.i_yError += State.yError * State.stepSize / 1000;
State.i_pError += State.pError * State.stepSize / 1000;
double d_yError =
(State.yError - State.yPrevError) / (State.stepSize / 1000);
double d_pError =
(State.pError - State.pPrevError) / (State.stepSize / 1000);
// PID Function
State.LQRx = (State.Kp * State.yError + State.Ki * State.i_yError +
State.Kd * d_yError);
State.LQRy = (State.Kp * State.pError + State.Ki * State.i_pError +
State.Kd * d_pError);
// std::cout << State.LQRx << ", ";
State.yPrevError = State.yError;
State.pPrevError = State.pError;
} else {
State.LQRx = 0;
State.LQRy = 0;
}
// LQR Force limiter X
if (State.LQRx > State.thrust)
State.LQRx = State.thrust;
else if (State.LQRx < -1 * State.thrust)
State.LQRx = -1 * State.thrust;
// LQR Force limiter Y
if (State.LQRy > State.thrust)
State.LQRy = State.thrust;
else if (State.LQRy < -1 * State.thrust)
State.LQRy = -1 * State.thrust;
}
void TVC(Vehicle &State) {
if (State.thrust < 1) {
// Define forces and moments for t = 0
@ -324,6 +374,9 @@ void state2vec(Vehicle &State, outVector &stateVector, int t) {
stateVector.servo2[t] = State.yServoDegs;
stateVector.thrustFiring[t] = State.thrustFiring;
stateVector.LQRx[t] = State.LQRx;
stateVector.LQRy[t] = State.LQRy;
}
void write2CSV(outVector &stateVector, Vehicle &State) {
@ -341,7 +394,7 @@ void write2CSV(outVector &stateVector, Vehicle &State) {
// Output file header. These are the variables that we output - useful for
// debugging
outfile << "t, x, y, z, vx, vy, vz, ax, ay, az, yaw, pitch, roll, yawdot, "
"pitchdot, rolldot, Servo1, Servo2, thrustFiring"
"pitchdot, rolldot, Servo1, Servo2, thrustFiring, LQRx, LQRy"
<< std::endl;
std::cout << "Writing to csv...\n";
@ -373,7 +426,10 @@ void write2CSV(outVector &stateVector, Vehicle &State) {
outfile << stateVector.servo1[t] << ", ";
outfile << stateVector.servo2[t] << ", ";
outfile << stateVector.thrustFiring[t] << std::endl;
outfile << stateVector.thrustFiring[t] << ", ";
outfile << stateVector.LQRx[t] << ", ";
outfile << stateVector.LQRy[t] << std::endl;
}
outfile.close();

7
input.csv
View File

@ -4,8 +4,8 @@ vz,0,m/s
yaw,5,degs
pitch,10,degs
roll,0,degs
yawdot,1,degs/s
pitchdot,-1,degs/s
yawdot,0,degs/s
pitchdot,0,degs/s
rolldot,0,degs/s
Max Servo Rotation,7.5,degs
Initial Mass,1.2,kg
@ -15,3 +15,6 @@ Vehicle Height,0.53,m
Vehicle Radius,0.05,m
Moment Arm,0.15,m
Sim Step Size,1,ms
Kp,-0.1,x
Ki,0.1, x
Kd,-0.5, x

1 vx 0 m/s
4 yaw 5 degs
5 pitch 10 degs
6 roll 0 degs
7 yawdot 1 0 degs/s
8 pitchdot -1 0 degs/s
9 rolldot 0 degs/s
10 Max Servo Rotation 7.5 degs
11 Initial Mass 1.2 kg
15 Vehicle Radius 0.05 m
16 Moment Arm 0.15 m
17 Sim Step Size 1 ms
18 Kp -0.1 x
19 Ki 0.1 x
20 Kd -0.5 x

82
src/main.cpp
View File

@ -3,9 +3,9 @@
#include <cmath>
#include <fstream>
#include <iostream>
#include <string>
#include <stdexcept> // std::runtime_error
#include <stdio.h>
#include <string>
#include <vector>
#include "Vehicle.h"
@ -27,59 +27,67 @@ int main() {
std::vector<double> varValueVec = std::vector<double>(17, 0.0);
std::string varName, varValue, varUnits;
for (int i; i < 17; i++) {
for (int i; i < 20; i++) {
std::getline(inFile, varName, ',');
std::getline(inFile, varValue, ',');
varValueVec[i] = stod(varValue);
std::getline(inFile, varUnits);
}
}
// Initial Velocity
State.vx = varValueVec[0]; // [m/s]
State.vy = varValueVec[1]; // [m/s]
State.vz = varValueVec[2]; // [m/s]
State.vx = varValueVec[0]; // [m/s]
State.vy = varValueVec[1]; // [m/s]
State.vz = varValueVec[2]; // [m/s]
// Initial YPR
State.yaw = varValueVec[3] * M_PI / 180; // [rad]
State.pitch = varValueVec[4] * M_PI / 180; // [rad]
State.roll = varValueVec[5] * M_PI / 180; // [rad]
// Initial YPR
State.yaw = varValueVec[3] * M_PI / 180; // [rad]
State.pitch = varValueVec[4] * M_PI / 180; // [rad]
State.roll = varValueVec[5] * M_PI / 180; // [rad]
// Initial YPRdot
State.yawdot = varValueVec[6] * M_PI / 180; // [rad/s]
State.pitchdot = varValueVec[7] * M_PI / 180; // [rad/s]
State.rolldot = varValueVec[8] * M_PI / 180; // [rad/s]
// Initial YPRdot
State.yawdot = varValueVec[6] * M_PI / 180; // [rad/s]
State.pitchdot = varValueVec[7] * M_PI / 180; // [rad/s]
State.rolldot = varValueVec[8] * M_PI / 180; // [rad/s]
// Servo Limitation
State.maxServo = varValueVec[9]; // [degs]
// Servo Limitation
State.maxServo = varValueVec[9]; // [degs]
// Vehicle Properties
State.massInitial = varValueVec[10]; // [kg]
State.vehicleHeight = varValueVec[13]; // [m]
State.vehicleRadius = varValueVec[14]; // [m]
State.momentArm = varValueVec[15]; // [m]
// Vehicle Properties
State.massInitial = varValueVec[10]; // [kg]
State.vehicleHeight = varValueVec[13]; // [m]
State.vehicleRadius = varValueVec[14]; // [m]
State.momentArm = varValueVec[15]; // [m]
// Sim Step Size
State.stepSize = varValueVec[16]; // [ms]
// Sim Step Size
State.stepSize = varValueVec[16]; // [ms]
// Other Properties
State.burntime = varValueVec[12]; // [s]
State.massPropellant = varValueVec[11]; // [kg]
State.massBurnout = State.massInitial - State.massPropellant; // [kg]
State.mdot = State.massPropellant / State.burntime; // [kg/s]
State.mass = State.massInitial; // [kg]
State.burnElapsed = 2000; // [s]
PrevState.thrust = 0; // [N]
// PID Gains
State.Kp = varValueVec[17];
State.Ki = varValueVec[18];
State.Kd = -1;
bool outcome = sim(State, PrevState);
std::cout << State.Kd << "\n";
std::cout << "Finished"
<< "\n";
// Other Properties
State.burntime = varValueVec[12]; // [s]
State.massPropellant = varValueVec[11]; // [kg]
State.massBurnout = State.massInitial - State.massPropellant; // [kg]
State.mdot = State.massPropellant / State.burntime; // [kg/s]
State.mass = State.massInitial; // [kg]
State.burnElapsed = 2000; // [s]
PrevState.thrust = 0; // [N]
if (outcome == 1) {
std::cout << "Sim Result = Success!";
return 0;
bool outcome = sim(State, PrevState);
std::cout << "Finished"
<< "\n";
if (outcome == 1) {
std::cout << "Sim Result = Success!";
return 0;
} else if (outcome == 0) {
std::cout << "Sim Result = Failed!";
// return 1; Until I figure out how to make CI/CD continue even when run
// fails.
return 0;