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

more cleanup

Browse Source
This commit is contained in:
bpmcgeeney
2021-09-21 22:14:10 -07:00
parent 9eb9bce77c
commit 1b13a88c7d
4 changed files with 16 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
include/Vehicle.h
View File

@@ -35,6 +35,7 @@ struct Vehicle {
double yError, yPrevError; double yError, yPrevError;
double pError, pPrevError; double pError, pPrevError;
double i_yError, i_pError = 0; double i_yError, i_pError = 0;
int pidTest = 0;
double simTime; double simTime;
int stepSize; int stepSize;

18
include/sim.h
View File

@@ -190,19 +190,25 @@ void pidController(Vehicle &State, struct Vehicle &PrevState) {
// Make sure we start reacting when we start burning // Make sure we start reacting when we start burning
if (State.thrust > 0.01) { if (State.thrust > 0.01) {
State.yError = State.yaw;
State.pError = State.pitch;
// Integral of Error // Integral of Error
State.i_yError = integral(State.yaw, State.i_yError, State.stepSize); State.i_yError = integral(State.yError, State.i_yError, State.stepSize);
State.i_pError = integral(State.pitch, State.i_pError, State.stepSize); State.i_pError = integral(State.pError, State.i_pError, State.stepSize);
// Derivative of Error // Derivative of Error
double d_yError = derivative(State.yaw, PrevState.yaw, State.stepSize); double d_yError =
double d_pError = derivative(State.pitch, PrevState.pitch, State.stepSize); derivative(State.yError, PrevState.yError, State.stepSize);
double d_pError =
derivative(State.pError, PrevState.pError, State.stepSize);
// PID Function - it says LQR but this is just so that it gets passed to the // PID Function - it says LQR but this is just so that it gets passed to the
// TVC block properly // TVC block properly
State.LQRx = (State.Kp * State.yaw + State.Ki * State.i_yError + State.LQRx = (State.Kp * State.yError + State.Ki * State.i_yError +
State.Kd * d_yError); State.Kd * d_yError);
State.LQRy = (State.Kp * State.pitch + State.Ki * State.i_pError + State.LQRy = (State.Kp * State.pError + State.Ki * State.i_pError +
State.Kd * d_pError); State.Kd * d_pError);
} else { } else {

6
input.csv
View File

@@ -15,6 +15,6 @@ Vehicle Height,0.53,m
Vehicle Radius,0.05,m Vehicle Radius,0.05,m
Moment Arm,0.15,m Moment Arm,0.15,m
Sim Step Size,1,ms Sim Step Size,1,ms
Kp,-0.25,x Kp,-1,x
Ki,0.4,x Ki,-1,x
Kd,1,x Kd,-1,x
1 vx 0 m/s
15 Vehicle Radius 0.05 m
16 Moment Arm 0.15 m
17 Sim Step Size 1 ms
18 Kp -0.25 -1 x
19 Ki 0.4 -1 x
20 Kd 1 -1 x

4
src/main.cpp
View File

@@ -66,10 +66,6 @@ int main() {
State.Ki = varValueVec[18]; State.Ki = varValueVec[18];
State.Kd = varValueVec[19]; State.Kd = varValueVec[19];
std::cout << State.Kp << "\n";
std::cout << State.Ki << "\n";
std::cout << State.Kd << "\n";
// Other Properties // Other Properties
State.burntime = varValueVec[12]; // [s] State.burntime = varValueVec[12]; // [s]
State.massPropellant = varValueVec[11]; // [kg] State.massPropellant = varValueVec[11]; // [kg]