diff --git a/README.md b/README.md
index 7add493..b06f1f1 100644
--- a/README.md
+++ b/README.md
@@ -3,11 +3,10 @@ A Proportional-Integral-Derivative controller to self balance a ball
Physics for the simulation is implemented according to [this paper](https://www.academia.edu/76867878/Swing_up_and_positioning_control_of_an_inverted_wheeled_cart_pendulum_system_with_chaotic_balancing_motions) (excluding the counter-balances and connecting rod)
-I used Range-Kutta method (4th order) to solve the system.
+I used Runge-Kutta method (4th order) to solve the system.
+
Camera dynamics are implemented with the help of [this](https://www.youtube.com/watch?v=KPoeNZZ6H4s) video
Use arrow keys to control the cart, and disturb the ball.
-
-
diff --git a/src/camera.rs b/src/camera.rs
index 965ec42..142a37f 100644
--- a/src/camera.rs
+++ b/src/camera.rs
@@ -9,6 +9,12 @@ pub struct CameraDynamics {
k3: f64,
}
+impl Default for CameraDynamics {
+ fn default() -> Self {
+ Self::new(1.5, 0.75, 0., 0.)
+ }
+}
+
impl CameraDynamics {
pub fn new(f: f64, z: f64, r: f64, init: f64) -> Self {
CameraDynamics {
diff --git a/src/cart.rs b/src/cart.rs
index 1016ed0..55edd53 100644
--- a/src/cart.rs
+++ b/src/cart.rs
@@ -4,7 +4,7 @@ use std::f64::consts::PI;
use macroquad::prelude::*;
-use crate::state::State;
+use crate::{camera::CameraDynamics, state::State};
#[derive(PartialEq, Eq)]
pub enum Integrator {
Euler,
@@ -38,6 +38,7 @@ pub struct Cart {
pub b1: f64,
pub b2: f64,
pub R: f64,
+ pub camera: CameraDynamics,
g: f64,
m1: f64,
m2: f64,
@@ -75,13 +76,14 @@ impl Default for Cart {
steps: 5,
enable: true,
integrator: Integrator::default(),
+ camera: CameraDynamics::default(),
}
}
}
impl Cart {
pub fn update(&mut self, dt: f64) {
- self.state.update_camera(dt);
+ self.camera.update(self.state.x, self.state.v, dt);
let steps = if dt > 0.02 {
(60. * dt) as i32
} else {
@@ -161,7 +163,7 @@ impl Cart {
pub fn display(&self, color: Color, thickness: f32, length: f32, depth: f32) {
draw_line(-length, -depth, length, -depth, thickness, color);
- let x = (self.state.x - self.state.camera.unwrap().y) as f32 * self.ui_scale;
+ let x = (self.state.x - self.camera.y) as f32 * self.ui_scale;
let R = self.R as f32 * self.ui_scale;
let (c, s) = (
(self.state.x / self.R).cos() as f32,
@@ -170,7 +172,7 @@ impl Cart {
let ticks = (9. / self.ui_scale) as i32;
let gap = 2. / ticks as f32;
- let offset = (self.state.camera.unwrap().y as f32 * self.ui_scale) % gap;
+ let offset = (self.camera.y as f32 * self.ui_scale) % gap;
for i in 0..ticks + 2 {
draw_line(
(-offset + gap * i as f32 - 1.) * length,
diff --git a/src/main.rs b/src/main.rs
index 999d190..773036f 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -24,8 +24,8 @@ async fn main() {
let grid = 0.15;
let w_init = 1280.;
let mut cart = Cart::default();
- let vingette = load_texture("vingette.png").await.unwrap();
- let font = load_ttf_font("Ubuntu-Regular.ttf").await.unwrap();
+ let vingette = Texture2D::from_file_with_format(include_bytes!("../vingette.png"), None);
+ let font = load_ttf_font_from_bytes(include_bytes!("../Ubuntu-Regular.ttf")).unwrap();
setup_theme();
let mut forceplt = Graph::new(
&["Force", "Thrust"],
diff --git a/src/state.rs b/src/state.rs
index 4d1e92e..c4f2003 100644
--- a/src/state.rs
+++ b/src/state.rs
@@ -9,7 +9,6 @@ pub struct State {
pub v: f64,
pub w: f64,
pub th: f64,
- pub camera: Option,
}
impl Default for State {
@@ -20,13 +19,7 @@ impl Default for State {
impl State {
pub fn from(x: f64, v: f64, w: f64, th: f64) -> Self {
- State {
- x,
- v,
- w,
- th,
- camera: Some(CameraDynamics::new(1.5, 0.75, 0., 0.0)),
- }
+ State { x, v, w, th }
}
pub fn update(&mut self, (vdot, v, wdot, w): (f64, f64, f64, f64), dt: f64) {
@@ -37,12 +30,6 @@ impl State {
self.x += v * dt;
}
- pub fn update_camera(&mut self, dt: f64) {
- if let Some(camera) = &mut self.camera {
- camera.update(self.x, self.v, dt);
- }
- }
-
pub fn after(&self, (vdot, v, wdot, w): (f64, f64, f64, f64), dt: f64) -> State {
let mut new_state = self.clone();
new_state.update((vdot, v, wdot, w), dt);
diff --git a/src/ui.rs b/src/ui.rs
index 402bbdb..44657a0 100644
--- a/src/ui.rs
+++ b/src/ui.rs
@@ -1,4 +1,4 @@
-use std::collections::VecDeque;
+use std::{collections::VecDeque, f32::INFINITY};
use egui::{
epaint::Shadow,
@@ -7,7 +7,11 @@ use egui::{
};
use macroquad::prelude::*;
-use crate::cart::{self, Cart};
+use crate::{
+ camera::CameraDynamics,
+ cart::{self, Cart},
+ state::State,
+};
pub struct Graph {
title: &'static [&'static str],
@@ -258,7 +262,11 @@ pub fn draw_ui(w: f32, grid: f32, cart: &mut Cart, forceplt: &mut Graph, forcepl
ui.label("L_rod");
});
ui.horizontal(|ui| {
- ui.add(DragValue::new(&mut cart.Fclamp).speed(1.));
+ ui.add(
+ DragValue::new(&mut cart.Fclamp)
+ .clamp_range(0.0..=INFINITY)
+ .speed(1.),
+ );
ui.label("F_clamp");
});
});
@@ -297,7 +305,11 @@ pub fn draw_ui(w: f32, grid: f32, cart: &mut Cart, forceplt: &mut Graph, forcepl
ui.label("R_wheel");
});
ui.horizontal(|ui| {
- ui.add(DragValue::new(&mut cart.Finp).speed(1.));
+ ui.add(
+ DragValue::new(&mut cart.Finp)
+ .clamp_range(0.0..=INFINITY)
+ .speed(1.),
+ );
ui.label("Input Force");
});
});
@@ -353,7 +365,11 @@ pub fn draw_ui(w: f32, grid: f32, cart: &mut Cart, forceplt: &mut Graph, forcepl
"Controller: OFF"
},
);
- egui::reset_button(ui, &mut cart.state);
+ if ui.button("Reset").clicked() {
+ cart.state = State::default();
+ cart.int = 0.;
+ cart.camera = CameraDynamics::default();
+ };
})
});
});