Implement successful swing-up control.

crates/lyapunov-demo/src/main.rs

@@ -4,54 +4,59 @@ use lyapunov::units::*;
 use lyapunov::{Controller, DCMotor, Integrator, LinearFeedback, Pendulum, Rk4};
 
 fn main() {
-    let motor = DCMotor::new(
+    let qube_servo_2 = DCMotor::new(
-        ElectricalResistance::new::<ohm>(1.0),
+        ElectricalResistance::new::<ohm>(8.4),
-        Inductance::new::<henry>(0.5),
+        Inductance::new::<henry>(0.00116),
         lyapunov::TorqueConstant::from(
-            Torque::new::<newton_meter>(0.5) / ElectricCurrent::new::<ampere>(1.0),
+            Torque::new::<newton_meter>(0.042) / ElectricCurrent::new::<ampere>(1.0),
         ),
         ElectricCurrent::new::<ampere>(2.0),
-        ElectricPotential::new::<volt>(12.0),
+        Voltage::new::<volt>(10.0),
     );
 
     let system = Pendulum::new(
-        Mass::new::<kilogram>(1.0),
+        Mass::new::<kilogram>(0.024),
-        Length::new::<meter>(1.0),
+        Length::new::<meter>(0.129),
-        MomentOfInertia::new::<kilogram_square_meter>(1.0),
+        MomentOfInertia::new::<kilogram_square_meter>(3.33 * 0.00001),
         Acceleration::new::<meter_per_second_squared>(9.81),
-        motor,
+        qube_servo_2,
     );
 
     // Gains are in A/rad and A/(rad/s).
-    let feedback = LinearFeedback::<2, 1>::from_gains([8.0, 2.0]);
+    let feedback = LinearFeedback::<2, 1>::from_gains([0.02, 0.008]);
 
     let rk4 = Rk4;
 
-    // Start displaced from the downward equilibrium.
+    // Start at the downward (stable) equilibrium to demonstrate swing-up.
     let mut x = ThetaOmegaState::new(
-        Angle::new::<radian>(0.5),
+        Angle::new::<radian>(0.0),
         AngularVelocity::new::<radian_per_second>(0.0),
     );
     let r = ThetaOmegaState::new(
-        Angle::new::<radian>(0.0),
+        Angle::new::<radian>(3.14159),
         AngularVelocity::new::<radian_per_second>(0.0),
     );
 
     let dt = Time::new::<millisecond>(10.0);
 
-    for step in 0..300 {
+    for step in 0..1_000 {
         let t = dt * (step as f64);
 
         let u_fb = feedback.calculate(&system, t, &x, &r);
-        let u = u_fb.saturate(&system);
+        let u_ff = system.gravity_cancelling_input(&x);
+        let u_total = u_fb + u_ff;
+        let u = u_total.saturate(&system);
+
         x = rk4.step(&system, t, &x, &u, dt);
 
         println!(
-            "t={:.2}s  theta={:.3?}  omega={:.3?}  current={:.3?}",
+            "t={:.2?}  theta={:.3?}  omega={:.3?}  current={:.3?}  u_fb={:.3?}  u_ff={:.3?}",
-            t.value,
+            t,
             x.angle(),
             x.angular_velocity(),
             u.current(),
+            u_fb.current(),
+            u_ff.current()
         );
     }
 }

crates/lyapunov/src/systems/pendulum.rs

@@ -30,6 +30,13 @@ impl Pendulum {
             motor,
         }
     }
+
+    pub fn gravity_cancelling_input(&self, x: &ThetaOmegaState) -> CurrentInput {
+        let theta = x.angle().value;
+        let current = self.mass.value * self.gravity.value * self.length.value * theta.sin()
+            / self.motor.torque_constant.value;
+        CurrentInput::new(ElectricCurrent::new::<ampere>(current))
+    }
 }
 
 impl System<2, 1> for Pendulum {