46 lines
1.3 KiB
Rust
46 lines
1.3 KiB
Rust
use lyapunov::states::ThetaOmegaState;
|
|
use lyapunov::traits::Saturate;
|
|
use lyapunov::units::*;
|
|
use lyapunov::{Controller, DCMotor, Integrator, LinearFeedback, Pendulum, Rk4};
|
|
|
|
fn main() {
|
|
let system = Pendulum::new(
|
|
Mass::new::<kilogram>(0.024),
|
|
Length::new::<meter>(0.129),
|
|
MomentOfInertia::new::<kilogram_square_meter>(3.33 * 0.00001),
|
|
Acceleration::new::<meter_per_second_squared>(9.81),
|
|
DCMotor::qube_servo_2(),
|
|
);
|
|
|
|
let r = ThetaOmegaState::new(Angle::new::<radian>(std::f64::consts::PI));
|
|
|
|
let feedback = LinearFeedback::<2, 1>::pendulum(&system);
|
|
|
|
let rk4 = Rk4;
|
|
|
|
// Start at the downward (stable) equilibrium to demonstrate swing-up.
|
|
let mut x = ThetaOmegaState::zero();
|
|
|
|
let dt = Time::new::<millisecond>(10.0);
|
|
|
|
for step in 0..350 {
|
|
let t = dt * (step as f64);
|
|
|
|
let u_fb = feedback.calculate(&system, t, &x, &r);
|
|
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?} theta={:.3?} omega={:.3?} current={:.3?} u_fb={:.3?} u_ff={:.3?}",
|
|
t,
|
|
x.angle(),
|
|
x.angular_velocity(),
|
|
u.current(),
|
|
u_fb.current(),
|
|
u_ff.current()
|
|
);
|
|
}
|
|
}
|