├── .gitignore ├── Cargo.toml ├── LICENSE ├── README.md ├── Ubuntu-Regular.ttf ├── index.html ├── pid-balancer.wasm ├── src ├── camera.rs ├── cart.rs ├── main.rs ├── state.rs ├── theme.rs └── ui.rs └── vingette.png /.gitignore: -------------------------------------------------------------------------------- 1 | # Generated by Cargo 2 | # will have compiled files and executables 3 | /target/ 4 | 5 | # Remove Cargo.lock from gitignore if creating an executable, leave it for libraries 6 | # More information here https://doc.rust-lang.org/cargo/guide/cargo-toml-vs-cargo-lock.html 7 | Cargo.lock 8 | 9 | # These are backup files generated by rustfmt 10 | **/*.rs.bk 11 | vingette2.png 12 | test.py 13 | 14 | # Added by cargo 15 | 16 | /target 17 | -------------------------------------------------------------------------------- /Cargo.toml: -------------------------------------------------------------------------------- 1 | [package] 2 | name = "pid-balancer" 3 | version = "0.1.0" 4 | edition = "2021" 5 | 6 | [dependencies] 7 | egui-macroquad = "0.15.0" 8 | macroquad = "0.3.25" 9 | 10 | [profile.release] 11 | opt-level = 'z' # Optimize for size 12 | lto = true # Enable link-time optimization 13 | codegen-units = 1 # Reduce number of codegen units to increase optimizations 14 | panic = 'abort' # Abort on panic 15 | strip = true # Strip symbols from binary* -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2023 Sparsh Goenka 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # PID Controller Simualation 2 | A Proportional-Integral-Derivative controller to self balance a ball on a rolling cart. Use arrow keys to control the cart, and disturb the ball. 3 | 4 | ## Try on Web 5 | https://sparshg.github.io/pid-balancer/ 6 | 7 | ## Downloads for Desktop 8 | 9 | Windows, Mac: https://github.com/sparshg/pid-balancer/releases 10 | 11 | (Should work on Linux too, didn't compile) 12 | 13 | 14 | ## Implementation Details 15 | 16 | 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) 17 | 18 | I used Runge-Kutta method (4th order) to solve the system. System's energy will remain almost constant when controller is off and there is no drag. 19 | 20 | Camera dynamics are implemented with the help of [this](https://www.youtube.com/watch?v=KPoeNZZ6H4s) video 21 | 22 | ![result](https://github.com/user-attachments/assets/7dae2fd2-5dac-48cd-b440-f2f13524ac41) 23 | -------------------------------------------------------------------------------- /Ubuntu-Regular.ttf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sparshg/pid-balancer/c9fbfc456f773f4796618f999f920f7b6d3370e4/Ubuntu-Regular.ttf -------------------------------------------------------------------------------- /index.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | PID Controller Simulation 6 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /pid-balancer.wasm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sparshg/pid-balancer/c9fbfc456f773f4796618f999f920f7b6d3370e4/pid-balancer.wasm -------------------------------------------------------------------------------- /src/camera.rs: -------------------------------------------------------------------------------- 1 | use std::f64::consts::PI; 2 | 3 | #[derive(Clone, Copy, PartialEq)] 4 | pub struct CameraDynamics { 5 | pub y: f64, 6 | yv: f64, 7 | k1: f64, 8 | k2: f64, 9 | k3: f64, 10 | } 11 | 12 | impl Default for CameraDynamics { 13 | fn default() -> Self { 14 | Self::new(1.5, 0.75, 0., 0.) 15 | } 16 | } 17 | 18 | impl CameraDynamics { 19 | pub fn new(f: f64, z: f64, r: f64, init: f64) -> Self { 20 | CameraDynamics { 21 | y: init, 22 | yv: 0., 23 | k1: z / (PI * f), 24 | k2: 0.25 / (PI * PI * f * f), 25 | k3: r * z * 0.5 / (PI * f), 26 | } 27 | } 28 | 29 | pub fn update(&mut self, x: f64, xv: f64, dt: f64) { 30 | let k2 = self 31 | .k2 32 | .max(self.k1 * dt) 33 | .max(0.5 * dt * dt + 0.5 * dt * self.k1); 34 | self.y += dt * self.yv; 35 | self.yv += dt * (x + self.k3 * xv - self.y - self.k1 * self.yv) / k2; 36 | } 37 | } 38 | -------------------------------------------------------------------------------- /src/cart.rs: -------------------------------------------------------------------------------- 1 | #![allow(non_snake_case)] 2 | 3 | use std::f64::consts::PI; 4 | 5 | use macroquad::prelude::*; 6 | 7 | use crate::{camera::CameraDynamics, state::State}; 8 | #[derive(PartialEq, Eq)] 9 | pub enum Integrator { 10 | Euler, 11 | RungeKutta4, 12 | } 13 | 14 | impl Default for Integrator { 15 | fn default() -> Self { 16 | Self::RungeKutta4 17 | } 18 | } 19 | 20 | #[derive(PartialEq)] 21 | pub struct Cart { 22 | pub F: f64, 23 | pub Fclamp: f64, 24 | pub Finp: f64, 25 | pub ui_scale: f32, 26 | pub enable: bool, 27 | pub pid: (f64, f64, f64), 28 | pub error: f64, 29 | pub int: f64, 30 | pub state: State, 31 | pub integrator: Integrator, 32 | pub steps: i32, 33 | pub m: f64, 34 | pub M: f64, 35 | pub mw: f64, 36 | pub ml: f64, 37 | pub l: f64, 38 | pub b1: f64, 39 | pub b2: f64, 40 | pub R: f64, 41 | pub camera: CameraDynamics, 42 | g: f64, 43 | m1: f64, 44 | m2: f64, 45 | m3: f64, 46 | } 47 | 48 | impl Default for Cart { 49 | fn default() -> Self { 50 | let (M, m, ml, mw) = (5., 0.5, 1., 1.); 51 | let m1 = m + M + ml + 3. * mw; 52 | let m2 = m + ml / 3.; 53 | let m3 = m + ml / 2.; 54 | 55 | Cart { 56 | m, 57 | M, 58 | l: 1., 59 | g: 9.80665, 60 | F: 0., 61 | Fclamp: 400., 62 | Finp: 20., 63 | int: 0., 64 | error: 0., 65 | R: 0.1, 66 | state: State::default(), 67 | b1: 0.01, 68 | b2: 0.005, 69 | ui_scale: 0.3, 70 | mw, 71 | ml, 72 | m1, 73 | m2, 74 | m3, 75 | pid: (40., 8., 2.5), 76 | steps: 5, 77 | enable: true, 78 | integrator: Integrator::default(), 79 | camera: CameraDynamics::default(), 80 | } 81 | } 82 | } 83 | 84 | impl Cart { 85 | pub fn update(&mut self, dt: f64) { 86 | self.camera.update(self.state.x, self.state.v, dt); 87 | let steps = if dt > 0.02 { 88 | ((self.steps * 60) as f64 * dt) as i32 89 | } else { 90 | self.steps 91 | }; 92 | let dt = dt / steps as f64; 93 | for _ in 0..steps { 94 | self.error = PI - self.state.th; 95 | self.int += self.error * dt; 96 | self.F = 0.; 97 | if self.enable { 98 | self.F = (10. 99 | * (self.error * self.pid.0 + self.int * self.pid.1 100 | - self.state.w * self.pid.2)) 101 | .clamp(-self.Fclamp, self.Fclamp); 102 | } 103 | if is_key_down(KeyCode::Left) { 104 | self.F = -self.Finp; 105 | self.int = 0. 106 | } else if is_key_down(KeyCode::Right) { 107 | self.F = self.Finp; 108 | self.int = 0. 109 | } 110 | let k1 = self.process_state(self.state); 111 | if self.integrator == Integrator::Euler { 112 | self.state.update(k1, dt); 113 | continue; 114 | } 115 | let k2 = self.process_state(self.state.after(k1, dt * 0.5)); 116 | let k3 = self.process_state(self.state.after(k2, dt * 0.5)); 117 | let k4 = self.process_state(self.state.after(k3, dt)); 118 | 119 | let k_avg = ( 120 | (k1.0 + 2.0 * k2.0 + 2.0 * k3.0 + k4.0) / 6.0, 121 | (k1.1 + 2.0 * k2.1 + 2.0 * k3.1 + k4.1) / 6.0, 122 | (k1.2 + 2.0 * k2.2 + 2.0 * k3.2 + k4.2) / 6.0, 123 | (k1.3 + 2.0 * k2.3 + 2.0 * k3.3 + k4.3) / 6.0, 124 | ); 125 | self.state.update(k_avg, dt); 126 | } 127 | } 128 | 129 | pub fn process_state(&self, state: State) -> (f64, f64, f64, f64) { 130 | let (_, v, w, th) = state.unpack(); 131 | 132 | let (s, c) = (th.sin(), th.cos()); 133 | let d = self.m2 * self.l * self.l * self.m1 - self.m3 * self.m3 * self.l * self.l * c * c; 134 | let f2 = -self.m3 * self.m3 * self.l * self.l * w * w * s * c 135 | + self.m3 * self.l * self.b1 * v * c 136 | - self.m1 * (self.m3 * self.g * self.l * s + self.b2 * w); 137 | let f4 = self.m2 * self.m3 * self.l * self.l * self.l * w * w * s 138 | - self.m2 * self.l * self.l * self.b1 * v 139 | + self.m3 * self.m3 * self.l * self.l * self.g * s * c 140 | + self.m3 * self.l * self.b2 * w * c; 141 | 142 | // returns (vdot, v, wdot, w) 143 | ( 144 | (f4 + self.m2 * self.l * self.l * self.F) / d, 145 | v, 146 | (f2 - self.m3 * self.l * c * self.F) / d, 147 | w, 148 | ) 149 | } 150 | 151 | pub fn get_potential_energy(&self) -> f64 { 152 | // with respect to ground 153 | -self.m3 * self.g * self.l * self.state.th.cos() 154 | } 155 | pub fn get_kinetic_energy(&self) -> f64 { 156 | 0.5 * self.m1 * self.state.v * self.state.v 157 | + 0.5 * self.m2 * self.state.w * self.state.w * self.l * self.l 158 | + self.m3 * self.state.v * self.state.w * self.l * self.state.th.cos() 159 | } 160 | pub fn get_total_energy(&self) -> f64 { 161 | self.get_potential_energy() + self.get_kinetic_energy() 162 | } 163 | 164 | pub fn display( 165 | &self, 166 | back_color: Color, 167 | color: Color, 168 | thickness: f32, 169 | length: f32, 170 | depth: f32, 171 | ) { 172 | draw_line(-length, -depth, length, -depth, thickness, color); 173 | let x = (self.state.x - self.camera.y) as f32 * self.ui_scale; 174 | let R = self.R as f32 * self.ui_scale; 175 | let (c, s) = ( 176 | (self.state.x / self.R).cos() as f32, 177 | (self.state.x / self.R).sin() as f32, 178 | ); 179 | 180 | let ticks = (9. / self.ui_scale) as i32; 181 | let gap = 2. / ticks as f32; 182 | let offset = (self.camera.y as f32 * self.ui_scale) % gap; 183 | for i in 0..ticks + 2 { 184 | draw_line( 185 | (-offset + gap * i as f32 - 1.) * length, 186 | -depth - 0.002, 187 | (-offset + gap * i as f32 - 1.) * length - 0.1 * self.ui_scale, 188 | -depth - 0.1 * self.ui_scale, 189 | thickness, 190 | color, 191 | ); 192 | } 193 | draw_rectangle( 194 | -1., 195 | -depth - 0.001, 196 | 1. - length - 0.003, 197 | -0.11 * self.ui_scale, 198 | back_color, 199 | ); 200 | draw_rectangle( 201 | length + 0.003, 202 | -depth - 0.001, 203 | 1. - length - 0.003, 204 | -0.11 * self.ui_scale, 205 | back_color, 206 | ); 207 | 208 | let (w, h) = (R * 10., R * 3.5); 209 | // cart 210 | draw_rectangle_lines(x - 0.5 * w, -depth + 2. * R, w, h, thickness * 2., color); 211 | 212 | // wheels 213 | draw_circle_lines(x - 0.30 * w, -depth + R, R, thickness, color); 214 | draw_circle_lines(x + 0.30 * w, -depth + R, R, thickness, color); 215 | draw_line( 216 | x - 0.30 * w, 217 | -depth + R, 218 | x - 0.30 * w - R * c, 219 | -depth + R + R * s, 220 | thickness, 221 | color, 222 | ); 223 | draw_line( 224 | x + 0.30 * w, 225 | -depth + R, 226 | x + 0.30 * w - R * c, 227 | -depth + R + R * s, 228 | thickness, 229 | color, 230 | ); 231 | 232 | let (c, s) = ((self.state.th).cos() as f32, (self.state.th).sin() as f32); 233 | let l = self.l as f32 * self.ui_scale; 234 | // pendulum 235 | draw_line( 236 | x, 237 | -depth + h + 2. * R, 238 | x + (l - R) * s, 239 | -depth + h + 2. * R - (l - R) * c, 240 | thickness, 241 | color, 242 | ); 243 | draw_circle_lines(x + l * s, -depth + h + 2. * R - l * c, R, thickness, color); 244 | draw_circle(x, -depth + 2. * R + h, 0.01, color); 245 | } 246 | } 247 | -------------------------------------------------------------------------------- /src/main.rs: -------------------------------------------------------------------------------- 1 | use crate::{theme::setup_theme, ui::Graph}; 2 | use cart::Cart; 3 | use egui::{pos2, Color32}; 4 | use egui_macroquad::egui; 5 | use macroquad::prelude::*; 6 | use ui::{draw_blue_grid, draw_speedometer, draw_ui, draw_vingette}; 7 | mod camera; 8 | mod cart; 9 | mod state; 10 | mod theme; 11 | mod ui; 12 | 13 | fn window_conf() -> Conf { 14 | Conf { 15 | window_title: "Cart".to_string(), 16 | fullscreen: true, 17 | // window_resizable: false, 18 | window_width: 1280, 19 | window_height: 720, 20 | ..Default::default() 21 | } 22 | } 23 | #[macroquad::main(window_conf)] 24 | async fn main() { 25 | let grid = 0.15; 26 | let w_init = 1280.; 27 | let mut cart = Cart::default(); 28 | let vingette = Texture2D::from_file_with_format(include_bytes!("../vingette.png"), None); 29 | let font = load_ttf_font_from_bytes(include_bytes!("../Ubuntu-Regular.ttf")).unwrap(); 30 | setup_theme(); 31 | let mut forceplt = Graph::new( 32 | &["Force", "Thrust"], 33 | pos2((0.5 - 2. * grid) * w_init, 0.), 34 | egui::vec2(1.5, 1.) * grid * w_init, 35 | None, 36 | ); 37 | let mut forceplt1 = Graph::new( 38 | &["PID", "Integral", "Derivative", "Error"], 39 | pos2((0.5 + 0.5 * grid) * w_init, 0.), 40 | egui::vec2(1.5, 1.) * grid * w_init, 41 | Some([Color32::WHITE, Color32::LIGHT_GREEN, Color32::LIGHT_RED].to_vec()), 42 | ); 43 | next_frame().await; 44 | let back_color = Color::new(0.00, 0.43, 0.95, 1.00); 45 | 46 | loop { 47 | set_camera(&Camera2D { 48 | zoom: vec2(1., screen_width() / screen_height()), 49 | ..Default::default() 50 | }); 51 | if is_key_pressed(KeyCode::Q) || is_key_pressed(KeyCode::Escape) { 52 | break; 53 | } 54 | if get_time() > 0. { 55 | cart.update(get_frame_time() as f64); 56 | } 57 | forceplt.update([cart.F].to_vec()); 58 | forceplt1.update([cart.int, -cart.state.w, cart.error].to_vec()); 59 | 60 | clear_background(back_color); 61 | draw_blue_grid(grid, SKYBLUE, 0.001, 3, 0.003); 62 | 63 | cart.display(back_color, WHITE, 0.006, 6. * grid, 3. * grid); 64 | draw_speedometer( 65 | &format!( 66 | "Angular Velocity ({}) {:.2}", 67 | if cart.state.w.is_sign_negative() { 68 | "-" 69 | } else { 70 | "+" 71 | }, 72 | cart.state.w.abs() 73 | ), 74 | vec2(0., screen_height() / screen_width() - 0.75 * grid), 75 | 0.08, 76 | cart.state.w as f32, 77 | 9., 78 | 0.8, 79 | font, 80 | 14., 81 | false, 82 | ); 83 | draw_speedometer( 84 | &format!( 85 | "Cart Velocity ({}) {:.2}", 86 | if cart.state.v.is_sign_negative() { 87 | "-" 88 | } else { 89 | "+" 90 | }, 91 | cart.state.v.abs() 92 | ), 93 | vec2(0., screen_height() / screen_width() - 1.75 * grid), 94 | 0.08, 95 | cart.state.v as f32, 96 | 20., 97 | 0.8, 98 | font, 99 | 14., 100 | true, 101 | ); 102 | draw_ui(w_init, grid, &mut cart, &mut forceplt, &mut forceplt1); 103 | draw_vingette(vingette); 104 | next_frame().await; 105 | } 106 | } 107 | -------------------------------------------------------------------------------- /src/state.rs: -------------------------------------------------------------------------------- 1 | use std::f64::consts::PI; 2 | 3 | #[derive(Clone, Copy, PartialEq)] 4 | 5 | pub struct State { 6 | pub x: f64, 7 | pub v: f64, 8 | pub w: f64, 9 | pub th: f64, 10 | } 11 | 12 | impl Default for State { 13 | fn default() -> Self { 14 | Self::from(0.0, 0.0, 0.0, PI + 0.5) 15 | } 16 | } 17 | 18 | impl State { 19 | pub fn from(x: f64, v: f64, w: f64, th: f64) -> Self { 20 | State { x, v, w, th } 21 | } 22 | 23 | pub fn update(&mut self, (vdot, v, wdot, w): (f64, f64, f64, f64), dt: f64) { 24 | self.w += wdot * dt; 25 | self.th += w * dt; 26 | self.th = (self.th % (2. * PI) + 2. * PI) % (2. * PI); 27 | self.v += vdot * dt; 28 | self.x += v * dt; 29 | } 30 | 31 | pub fn after(&self, (vdot, v, wdot, w): (f64, f64, f64, f64), dt: f64) -> State { 32 | let mut new_state = self.clone(); 33 | new_state.update((vdot, v, wdot, w), dt); 34 | new_state 35 | } 36 | 37 | pub fn unpack(&self) -> (f64, f64, f64, f64) { 38 | (self.x, self.v, self.w, self.th) 39 | } 40 | } 41 | -------------------------------------------------------------------------------- /src/theme.rs: -------------------------------------------------------------------------------- 1 | use egui::{epaint, style, Color32, Context, Rounding}; 2 | use egui_macroquad::egui; 3 | 4 | pub fn setup_theme() { 5 | egui_macroquad::cfg(|ctx| { 6 | get_theme(ctx); 7 | }); 8 | } 9 | 10 | fn get_theme(ctx: &Context) { 11 | let bg = Color32::from_black_alpha(30); 12 | let act = style::WidgetVisuals { 13 | bg_fill: bg, 14 | bg_stroke: epaint::Stroke { 15 | width: 0.5, 16 | color: Color32::WHITE, 17 | }, 18 | fg_stroke: epaint::Stroke { 19 | width: 0.5, 20 | color: Color32::WHITE, 21 | }, 22 | weak_bg_fill: Color32::TRANSPARENT, 23 | rounding: Rounding::from(2.), 24 | expansion: 0.0, 25 | }; 26 | let ina = style::WidgetVisuals { 27 | bg_fill: Color32::TRANSPARENT, 28 | bg_stroke: epaint::Stroke::NONE, 29 | fg_stroke: epaint::Stroke::NONE, 30 | weak_bg_fill: Color32::TRANSPARENT, 31 | rounding: Rounding::none(), 32 | 33 | expansion: 0.0, 34 | }; 35 | ctx.set_visuals(egui::Visuals { 36 | dark_mode: false, 37 | window_shadow: epaint::Shadow::NONE, 38 | panel_fill: Color32::TRANSPARENT, 39 | window_fill: Color32::from_black_alpha(50), 40 | override_text_color: Some(Color32::WHITE), 41 | window_stroke: epaint::Stroke::NONE, 42 | faint_bg_color: Color32::TRANSPARENT, 43 | extreme_bg_color: bg, 44 | widgets: style::Widgets { 45 | noninteractive: ina, 46 | inactive: act, 47 | hovered: act, 48 | active: act, 49 | open: act, 50 | }, 51 | ..Default::default() 52 | }); 53 | // ctx.set_visuals(style::Visuals::dark()); 54 | } 55 | -------------------------------------------------------------------------------- /src/ui.rs: -------------------------------------------------------------------------------- 1 | use egui_macroquad::egui; 2 | use std::{collections::VecDeque, f32::INFINITY}; 3 | 4 | use egui::{ 5 | epaint::Shadow, 6 | plot::{CoordinatesFormatter, Corner, HLine, Legend, Line, Plot, PlotBounds, PlotPoints}, 7 | Align, Align2, Color32, Context, DragValue, Frame, Layout, Pos2, Slider, Vec2, 8 | }; 9 | use macroquad::prelude::*; 10 | 11 | use crate::{ 12 | camera::CameraDynamics, 13 | cart::{self, Cart}, 14 | state::State, 15 | }; 16 | 17 | pub struct Graph { 18 | title: &'static [&'static str], 19 | pos: Pos2, 20 | size: Vec2, 21 | history: Vec>, 22 | hsize: usize, 23 | colors: Vec, 24 | } 25 | 26 | impl Graph { 27 | pub fn new( 28 | title: &'static [&'static str], 29 | pos: Pos2, 30 | size: egui::Vec2, 31 | colors: Option>, 32 | ) -> Self { 33 | if let Some(colors) = &colors { 34 | assert!(title.len() == colors.len() + 1); 35 | } 36 | Graph { 37 | title, 38 | pos, 39 | size, 40 | history: (0..title.len() - 1).map(|_| VecDeque::new()).collect(), 41 | hsize: 100, 42 | colors: colors 43 | .unwrap_or_else(|| (0..title.len() - 1).map(|_| Color32::WHITE).collect()), 44 | } 45 | } 46 | 47 | pub fn y(&mut self, y: f32) { 48 | self.pos.y = y; 49 | } 50 | 51 | pub fn update(&mut self, track: Vec) { 52 | assert!(track.len() == self.history.len()); 53 | for (i, &v) in track.iter().enumerate() { 54 | self.history[i].push_back(v as f32); 55 | if self.history[i].len() > self.hsize { 56 | self.history[i].pop_front(); 57 | } 58 | } 59 | } 60 | 61 | pub fn draw(&self, ctx: &Context, clamp: f64) { 62 | egui::Window::new(self.title[0]) 63 | .frame(Frame { 64 | inner_margin: egui::Margin::same(0.), 65 | outer_margin: egui::Margin::same(0.), 66 | rounding: egui::Rounding::none(), 67 | fill: Color32::TRANSPARENT, 68 | shadow: Shadow::NONE, 69 | stroke: egui::Stroke::new(2., Color32::WHITE), 70 | }) 71 | .current_pos(self.pos) 72 | .default_size(self.size) 73 | .resizable(false) 74 | .movable(false) 75 | .collapsible(false) 76 | .title_bar(false) 77 | .show(ctx, |ui| { 78 | Plot::new("example") 79 | .width(self.size.x) 80 | .height(self.size.y) 81 | .show_axes([false, false]) 82 | .show_background(false) 83 | .allow_drag(false) 84 | .allow_zoom(false) 85 | .allow_scroll(false) 86 | .allow_boxed_zoom(false) 87 | .show_x(false) 88 | .show_y(false) 89 | .coordinates_formatter( 90 | Corner::LeftBottom, 91 | CoordinatesFormatter::new(|&point, _| format!("y: {:.3}", point.y)), 92 | ) 93 | .legend(Legend::default().position(egui::plot::Corner::RightBottom)) 94 | .show(ui, |plot_ui| { 95 | plot_ui.set_plot_bounds(PlotBounds::from_min_max( 96 | [0., -clamp * 1.1], 97 | [self.hsize as f64, clamp * 1.1], 98 | )); 99 | plot_ui.hline(HLine::new(0.).color(Color32::WHITE).width(1.)); 100 | for i in 0..self.history.len() { 101 | plot_ui.line( 102 | Line::new( 103 | self.history[i] 104 | .iter() 105 | .enumerate() 106 | .map(|(i, &y)| [i as f64, y as f64]) 107 | .collect::(), 108 | ) 109 | .width(2.) 110 | .color(self.colors[i]) 111 | .name(self.title[i + 1]), 112 | ); 113 | } 114 | }) 115 | .response 116 | }); 117 | } 118 | } 119 | 120 | pub fn draw_speedometer( 121 | label: &str, 122 | center: macroquad::math::Vec2, 123 | radius: f32, 124 | speed: f32, 125 | max_speed: f32, 126 | extent: f32, 127 | font: Font, 128 | fsize: f32, 129 | oneway: bool, 130 | ) { 131 | let angle = if oneway { 132 | 0.5 * (1. + extent) - speed.abs() / max_speed * extent 133 | } else { 134 | 0.5 * (1. - speed / max_speed * extent) 135 | } * std::f32::consts::PI; 136 | let x = center.x + 0.8 * radius * angle.cos(); 137 | let y = center.y + 0.8 * radius * angle.sin(); 138 | let sides = 20; 139 | 140 | for i in 0..sides { 141 | let t = i as f32 / sides as f32; 142 | let rx = ((t * extent + 0.5 - 0.5 * extent) * std::f32::consts::PI).cos(); 143 | let ry = ((t * extent + 0.5 - 0.5 * extent) * std::f32::consts::PI).sin(); 144 | 145 | let p0 = vec2(center.x + radius * rx, center.y + radius * ry); 146 | let p00 = vec2(center.x + 1.1 * radius * rx, center.y + 1.1 * radius * ry); 147 | 148 | let rx = (((i + 1) as f32 / sides as f32 * extent + 0.5 - 0.5 * extent) 149 | * std::f32::consts::PI) 150 | .cos(); 151 | let ry = (((i + 1) as f32 / sides as f32 * extent + 0.5 - 0.5 * extent) 152 | * std::f32::consts::PI) 153 | .sin(); 154 | 155 | let p1 = vec2(center.x + radius * rx, center.y + radius * ry); 156 | let p11 = vec2(center.x + 1.1 * radius * rx, center.y + 1.1 * radius * ry); 157 | draw_line(p00.x, p00.y, p11.x, p11.y, 0.006, WHITE); 158 | draw_line( 159 | p0.x, 160 | p0.y, 161 | p1.x, 162 | p1.y, 163 | 0.004 164 | * if oneway { 165 | 1. - t 166 | } else { 167 | 3. * t * t - 3. * t + 1. 168 | }, 169 | WHITE, 170 | ); 171 | } 172 | push_camera_state(); 173 | set_default_camera(); 174 | let size = measure_text(label, None, fsize as u16, 1.); 175 | draw_text_ex( 176 | label, 177 | (0.5 + center.x) * screen_width() - size.width * 0.5, 178 | 0.5 * (screen_height() - center.y * screen_width()) + size.offset_y + size.height, 179 | TextParams { 180 | font: font, 181 | font_size: fsize as u16 * 2, 182 | font_scale: 0.5, 183 | color: Color::new(1., 1., 1., 0.75), 184 | ..Default::default() 185 | }, 186 | ); 187 | pop_camera_state(); 188 | let n = vec2(center.y - y, x - center.x); 189 | draw_triangle( 190 | vec2(center.x, center.y) + n * 0.08, 191 | vec2(center.x, center.y) - n * 0.08, 192 | vec2(x, y), 193 | WHITE, 194 | ) 195 | } 196 | pub fn draw_ui(w: f32, grid: f32, cart: &mut Cart, forceplt: &mut Graph, forceplt1: &mut Graph) { 197 | egui_macroquad::ui(|ctx| { 198 | // ctx.set_debug_on_hover(true); 199 | ctx.set_pixels_per_point(screen_width() / w); 200 | forceplt.y(2.); 201 | forceplt1.y(2.); 202 | egui::Window::new("Controls") 203 | .anchor(Align2::RIGHT_TOP, egui::emath::vec2(0., 0.)) 204 | .pivot(Align2::RIGHT_TOP) 205 | .default_width(1.25 * grid * w + 4.) 206 | .resizable(false) 207 | .movable(false) 208 | .collapsible(false) 209 | .title_bar(false) 210 | .show(ctx, |ui| { 211 | ui.with_layout(Layout::top_down(Align::RIGHT), |ui| { 212 | ui.add( 213 | Slider::new(&mut cart.pid.0, 0.0..=150.0) 214 | .drag_value_speed(0.2) 215 | .text("P"), 216 | ); 217 | ui.add( 218 | Slider::new(&mut cart.pid.1, 0.0..=100.0) 219 | .drag_value_speed(0.1) 220 | .text("I"), 221 | ); 222 | ui.add( 223 | Slider::new(&mut cart.pid.2, 0.0..=40.) 224 | .drag_value_speed(0.04) 225 | .text("D"), 226 | ); 227 | }); 228 | ui.separator(); 229 | ui.separator(); 230 | ui.columns(2, |cols| { 231 | cols[0].with_layout(Layout::top_down(Align::Max), |ui| { 232 | ui.horizontal(|ui| { 233 | ui.add( 234 | DragValue::new(&mut cart.M) 235 | .clamp_range(0.0..=100.) 236 | .speed(0.05), 237 | ); 238 | ui.label("M_cart"); 239 | }); 240 | ui.horizontal(|ui| { 241 | ui.add( 242 | DragValue::new(&mut cart.ml) 243 | .clamp_range(0.0..=100.) 244 | .speed(0.05), 245 | ); 246 | ui.label("M_rod"); 247 | }); 248 | ui.horizontal(|ui| { 249 | ui.add( 250 | DragValue::new(&mut cart.b1) 251 | .clamp_range(0.0..=0.5) 252 | .speed(0.0002) 253 | .custom_formatter(|x, _| format!("{:.3}", x)), 254 | ); 255 | ui.label("Drag"); 256 | }); 257 | ui.horizontal(|ui| { 258 | ui.add( 259 | DragValue::new(&mut cart.l) 260 | .clamp_range(0.1..=10.) 261 | .speed(0.05), 262 | ); 263 | ui.label("L_rod"); 264 | }); 265 | ui.horizontal(|ui| { 266 | ui.add( 267 | DragValue::new(&mut cart.Fclamp) 268 | .clamp_range(0.0..=INFINITY) 269 | .speed(1.), 270 | ); 271 | ui.label("F_clamp"); 272 | }); 273 | }); 274 | cols[1].with_layout(Layout::top_down(Align::Max), |ui| { 275 | ui.horizontal(|ui| { 276 | ui.add( 277 | DragValue::new(&mut cart.m) 278 | .clamp_range(0.0..=100.) 279 | .speed(0.05), 280 | ); 281 | ui.label("M_bob"); 282 | }); 283 | ui.horizontal(|ui| { 284 | ui.add( 285 | DragValue::new(&mut cart.mw) 286 | .clamp_range(0.0..=100.) 287 | .speed(0.05), 288 | ); 289 | ui.label("M_wheel"); 290 | }); 291 | ui.horizontal(|ui| { 292 | ui.add( 293 | DragValue::new(&mut cart.b2) 294 | .clamp_range(0.0..=0.5) 295 | .speed(0.0002) 296 | .custom_formatter(|x, _| format!("{:.3}", x)), 297 | ); 298 | ui.label("Ang_Drag"); 299 | }); 300 | ui.horizontal(|ui| { 301 | ui.add( 302 | DragValue::new(&mut cart.R) 303 | .clamp_range(0.0..=1.) 304 | .speed(0.005), 305 | ); 306 | ui.label("R_wheel"); 307 | }); 308 | ui.horizontal(|ui| { 309 | ui.add( 310 | DragValue::new(&mut cart.Finp) 311 | .clamp_range(0.0..=INFINITY) 312 | .speed(1.), 313 | ); 314 | ui.label("Input Force"); 315 | }); 316 | }); 317 | }); 318 | }); 319 | 320 | egui::Window::new("Physics") 321 | .anchor(Align2::LEFT_TOP, egui::emath::vec2(0., 0.)) 322 | .default_width(1.25 * grid * w + 2.) 323 | .resizable(false) 324 | .movable(false) 325 | .collapsible(false) 326 | // .title_bar(false) 327 | .show(ctx, |ui| { 328 | ui.with_layout(Layout::top_down(Align::Center), |ui| { 329 | ui.label(format!("System Energy: {:.2}", cart.get_total_energy())); 330 | ui.label(format!("Kinetic Energy: {:.2}", cart.get_kinetic_energy())); 331 | ui.label(format!( 332 | "Potential Energy: {:.2}", 333 | cart.get_potential_energy() 334 | )); 335 | ui.separator(); 336 | ui.horizontal(|ui| { 337 | ui.label("Integrator: "); 338 | ui.selectable_value(&mut cart.integrator, cart::Integrator::Euler, "Euler"); 339 | ui.selectable_value( 340 | &mut cart.integrator, 341 | cart::Integrator::RungeKutta4, 342 | "Runge-Kutta⁴", 343 | ); 344 | }); 345 | ui.separator(); 346 | ui.add( 347 | Slider::new(&mut cart.steps, 1..=100) 348 | .logarithmic(true) 349 | .text("Steps / Frame"), 350 | ); 351 | ui.add( 352 | Slider::new(&mut cart.ui_scale, 0.03..=0.6) 353 | .custom_formatter(|n, _| format!("{:.2}", n / 0.3)) 354 | .custom_parser(|s| s.parse::().map(|v| v * 0.3).ok()) 355 | .text("Draw Scale"), 356 | ); 357 | ui.separator(); 358 | ui.horizontal(|ui| { 359 | let enable = cart.enable; 360 | ui.label("System Controls:"); 361 | ui.toggle_value( 362 | &mut cart.enable, 363 | if enable { 364 | "Controller: ON" 365 | } else { 366 | "Controller: OFF" 367 | }, 368 | ); 369 | if ui.button("Reset").clicked() { 370 | cart.state = State::default(); 371 | cart.int = 0.; 372 | cart.camera = CameraDynamics::default(); 373 | }; 374 | }) 375 | }); 376 | }); 377 | forceplt.draw(ctx, cart.Fclamp); 378 | forceplt1.draw(ctx, 9.); 379 | }); 380 | egui_macroquad::draw(); 381 | } 382 | 383 | pub fn draw_blue_grid(grid: f32, color: Color, thickness: f32, bold_every: i32, bold_thick: f32) { 384 | draw_line(0., -1., 0., 1., bold_thick, color); 385 | draw_line(-1., 0., 1., 0., bold_thick, color); 386 | for i in 1..=(1. / grid as f32) as i32 { 387 | let thickness = if i % bold_every == 0 { 388 | bold_thick 389 | } else { 390 | thickness 391 | }; 392 | draw_line(i as f32 * grid, -1., i as f32 * grid, 1., thickness, color); 393 | draw_line( 394 | -i as f32 * grid, 395 | -1., 396 | -i as f32 * grid, 397 | 1., 398 | thickness, 399 | color, 400 | ); 401 | draw_line(-1., i as f32 * grid, 1., i as f32 * grid, thickness, color); 402 | draw_line( 403 | -1., 404 | -i as f32 * grid, 405 | 1., 406 | -i as f32 * grid, 407 | thickness, 408 | color, 409 | ); 410 | } 411 | } 412 | 413 | pub fn draw_vingette(tex: Texture2D) { 414 | set_default_camera(); 415 | draw_texture_ex( 416 | tex, 417 | 0., 418 | 0., 419 | WHITE, 420 | DrawTextureParams { 421 | dest_size: Some(vec2(screen_width(), screen_height())), 422 | ..Default::default() 423 | }, 424 | ); 425 | } 426 | -------------------------------------------------------------------------------- /vingette.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sparshg/pid-balancer/c9fbfc456f773f4796618f999f920f7b6d3370e4/vingette.png --------------------------------------------------------------------------------