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

Merge branch '11-servos-need-a-rate-limitation' into 'main'

Browse Source 
Resolve "Servos need a rate limitation"

Closes #11

See merge request lander-team/lander-cpp!8
This commit is contained in:
Brendan McGeeney 2021-10-15 19:49:45 +00:00
commit 016ddc1261
3 changed files with 49 additions and 33 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;
int maxServo;
int maxServoRate;
double xServoDegs, yServoDegs;
double xServoDegsDot, yServoDegsDot;
double Kp, Ki, Kd;
double yError, yPrevError;

70
include/sim.h
View File

@ -6,15 +6,16 @@
void burnStartTimeCalc(struct Vehicle &);
void thrustSelection(struct Vehicle &, int t);
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 state2vec(struct Vehicle &, struct Vehicle &, struct outVector &, int t);
void write2CSV(struct outVector &, struct Vehicle &);
double derivative(double x2, double x1, double dt);
double integral(double x2, double x1, double dt);
double limit(double value, double upr, double lwr);
// Any parameters that are constants should be declared here instead of buried
// in code
// Any parameters that are constants should be declared here instead of
// buried in code
double const dt = 0.01;
double const g = -9.81;
@ -32,7 +33,7 @@ bool sim(struct Vehicle &State, struct Vehicle &PrevState) {
vehicleDynamics(State, PrevState, t);
thrustSelection(State, t);
pidController(State, PrevState);
TVC(State);
TVC(State, PrevState);
state2vec(State, PrevState, stateVector, t);
t += State.stepSize;
@ -246,20 +247,12 @@ void pidController(Vehicle &State, struct Vehicle &PrevState) {
State.PIDy = 0;
}
// PID Force limiter X
if (State.PIDx > State.thrust)
State.PIDx = 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;
// PID Force Limiter
State.PIDx = limit(State.PIDx, State.thrust, -State.thrust);
State.PIDy = limit(State.PIDy, State.thrust, -State.thrust);
}
void TVC(Vehicle &State) {
void TVC(Vehicle &State, Vehicle &PrevState) {
if (State.thrust < 0.1) {
// Define forces and moments for t = 0
State.Fx = 0;
@ -273,21 +266,29 @@ void TVC(Vehicle &State) {
} else {
// Convert servo position to degrees for comparison to max allowable
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);
// Servo position limiter
if (State.yServoDegs > State.maxServo)
State.yServoDegs = State.maxServo;
else if (State.yServoDegs < -1 * State.maxServo)
State.yServoDegs = -1 * State.maxServo;
// Limit Servo Position
State.xServoDegs = limit(State.xServoDegs, State.maxServo, -State.maxServo);
State.yServoDegs = limit(State.yServoDegs, State.maxServo, -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
State.Fx = State.thrust * sin(State.xServoDegs * (M_PI / 180));
@ -401,3 +402,14 @@ double derivative(double current, double previous, double step) {
double integral(double currentChange, double prevValue, double dt) {
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;
}

6
src/main.cpp
View File

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