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

Resolve "Servos need a rate limitation"

Closes #11

See merge request lander-team/lander-cpp!8

include/Vehicle.h

@@ -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;

include/sim.h

@@ -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
+// Any parameters that are constants should be declared here instead of
-// in code
+// 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
+  // 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) {
     // 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
+    // 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
     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;
+}

src/main.cpp

@@ -28,9 +28,10 @@ int main() {
   State.vz = 0; // [m/s]
 
   // 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
   State.yawdot = 1 * M_PI / 180;    // [rad/s]
@@ -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]