Make the thing

.gitignore

@@ -1,14 +1 @@
-# Generated by Cargo
+/target
-# will have compiled files and executables
-debug/
-target/
-
-# Remove Cargo.lock from gitignore if creating an executable, leave it for libraries
-# More information here https://doc.rust-lang.org/cargo/guide/cargo-toml-vs-cargo-lock.html
-Cargo.lock
-
-# These are backup files generated by rustfmt
-**/*.rs.bk
-
-# MSVC Windows builds of rustc generate these, which store debugging information
-*.pdb

.idea/.gitignore

@@ -0,0 +1,5 @@
+# Default ignored files
+/shelf/
+/workspace.xml
+# Editor-based HTTP Client requests
+/httpRequests/

.idea/modules.xml

@@ -0,0 +1,8 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="ProjectModuleManager">
+    <modules>
+      <module fileurl="file://$PROJECT_DIR$/.idea/rust-window-thing.iml" filepath="$PROJECT_DIR$/.idea/rust-window-thing.iml" />
+    </modules>
+  </component>
+</project>

.idea/rust-window-thing.iml

@@ -0,0 +1,11 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<module type="EMPTY_MODULE" version="4">
+  <component name="NewModuleRootManager">
+    <content url="file://$MODULE_DIR$">
+      <sourceFolder url="file://$MODULE_DIR$/src" isTestSource="false" />
+      <excludeFolder url="file://$MODULE_DIR$/target" />
+    </content>
+    <orderEntry type="inheritedJdk" />
+    <orderEntry type="sourceFolder" forTests="false" />
+  </component>
+</module>

.idea/vcs.xml

@@ -0,0 +1,6 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="VcsDirectoryMappings">
+    <mapping directory="" vcs="Git" />
+  </component>
+</project>

Cargo.toml

@@ -5,3 +5,5 @@ edition = "2021"
 
 [dependencies]
 piston_window = "0.132.0"
+tokio = { version = "1.39.2", features = ["rt", "rt-multi-thread", "macros"] }
+find_folder = "*"

src/main.rs

@@ -1,44 +1,310 @@
 mod swerve_math;
 mod swervemodule;
+mod pid;
 
 extern crate piston_window;
 
+use std::thread;
+use std::time::Duration;
 use piston_window::*;
+use piston_window::math::Scalar;
 use piston_window::rectangle::square;
+use piston_window::types::Color;
+use crate::swerve_math::{degrees_to_radians, get_heading, get_heading_x, get_heading_y, Vector2d};
+use crate::swervemodule::SwerveModule;
 
-fn main() {
+
-    let mut window: PistonWindow = WindowSettings::new("Line Drawing", [512, 512])
+static SCREEN_SQUARE_SIZE:u32 = 1024;
+static SWERVE_CENTER_DISTANCE:u32 = SCREEN_SQUARE_SIZE/8;
+
+static XY_ACCEL:f32 = 0.2;
+static ROT_ACCEL:f32 = 0.001;
+static MAX_ROT_VEL:f32 = 32.;
+
+static VECTOR_SCALE:f32 = 20.;
+
+static XY_FRICTION:f32 = 0.95;
+static ROT_FRICTION:f32 = 0.998;
+// static mut TIME: u64 = 0;
+
+
+struct but {
+    key_w: bool,
+    key_a: bool,
+    key_s: bool,
+    key_d: bool,
+
+    key_q: bool,
+    key_e: bool,
+}
+
+static mut BUTTONS: but = but {
+    key_w: false, key_a: false, key_s: false, key_d: false, key_q: false, key_e: false,
+};
+
+static mut VEL_X: f32 = 0.;
+static mut VEL_Y: f32 = 0.;
+static mut VEL_R: f32 = 0.;
+
+static mut POS_X: f32 = 0.;
+static mut POS_Y: f32 = 0.;
+static mut POS_R: f32 = 0.;
+
+static mut W1:SwerveModule = SwerveModule::new(1);
+static mut W2:SwerveModule = SwerveModule::new(2);
+static mut W3:SwerveModule = SwerveModule::new(3);
+static mut W4:SwerveModule = SwerveModule::new(4);
+
+
+
+
+static mut TARGET_R: f32 = 0.;
+static mut TARGET_R_DELTA: f32 = 2.;
+
+#[tokio::main]
+async fn main() {
+
+    let mut window: PistonWindow = WindowSettings::new("Line Drawing", [SCREEN_SQUARE_SIZE, SCREEN_SQUARE_SIZE])
         .exit_on_esc(true)
         .build()
         .unwrap();
 
+    let assets = find_folder::Search::ParentsThenKids(3, 3)
+        .for_folder("assets").unwrap();
+    let ref font = assets.join("UbuntuMonoNerdFont-Regular.ttf");
+    let mut glyphs = window.load_font(font).unwrap();
 
 
-    while let Some(event) = window.next() {
+    fn draw_vertical_line(pos_x: f32, c: &Context, mut g: &mut G2d){
-        if let Some(Button::Keyboard(key)) = event.press_args() {
+        let mut x = -pos_x % SCREEN_SQUARE_SIZE as f32;
-            println!("Key pressed: {:?}", key);
+        if x < 0. {x += SCREEN_SQUARE_SIZE as f32}
-            if key.code() == 113 {break;}
+
+        line(color::GRAY, 1., [
+            x as f64,
+            0.,
+            x as f64,
+            SCREEN_SQUARE_SIZE as f64,
+        ], c.transform, g);
     }
 
-        if let Some(Button::Mouse(key)) = event.press_args() {
+    fn draw_horizontal_line(pos_y: f32, c: &Context, mut g: &mut G2d){
-            println!("Key pressed: {:?}", key);
+        let mut y = -pos_y % SCREEN_SQUARE_SIZE as f32;
+        if y < 0. {y += SCREEN_SQUARE_SIZE as f32}
+
+        line(color::GRAY, 1., [
+            0.,
+            y as f64,
+            SCREEN_SQUARE_SIZE as f64,
+            y as f64,
+        ], c.transform, g);
     }
 
-        window.draw_2d(&event, |c, g, _d| {
+    async unsafe fn update() {
-            clear(color::BLACK, g);
+        // let mut pid_x: pid::PIDController = pid::PIDController::new(
-
+        //     2.,
-            // Ellipse::new_border(color::WHITE, 1.0).draw(
+        //     0.1,
-            //     rectangle::centered_square(10.0, 10.0, 50.0),
+        //     9.,
-            //     &c.draw_state,
+        //     0.
-            //     c.transform,
+        // );
-            //     g,
+        //
+        // let mut pid_y: pid::PIDController = pid::PIDController::new(
+        //     2.,
+        //     0.1,
+        //     9.,
+        //     0.
         // );
 
-            swervemodule::SwerveModule::draw(&c,);
+        let mut pid_rot: pid::PIDController = pid::PIDController::new(
-
+            5.0,
-            Rectangle::new_border(color::WHITE, 1.0).draw(square(0.0, 0.0, 512.0), &c.draw_state, c.transform, g);
+            0.1,
+            100.,
+            0.
+        );
 
 
+        loop {
+            if BUTTONS.key_w {
+                VEL_Y -= (XY_ACCEL);
+            } else if BUTTONS.key_s {
+                VEL_Y += (XY_ACCEL);
+            }
+
+            if BUTTONS.key_a {
+                VEL_X -= (XY_ACCEL);
+            } else if BUTTONS.key_d {
+                VEL_X += (XY_ACCEL);
+            }
+
+            if BUTTONS.key_q {
+                TARGET_R -= TARGET_R_DELTA;
+                pid_rot.set_setpoint(TARGET_R as f64);
+            } else if BUTTONS.key_e {
+                TARGET_R += TARGET_R_DELTA;
+                pid_rot.set_setpoint(TARGET_R as f64);
+            }
+
+
+
+
+
+            VEL_R += ROT_ACCEL*pid_rot.update(POS_R as f64, 1.) as f32;
+
+
+            VEL_R = f32::clamp(VEL_R, -MAX_ROT_VEL, MAX_ROT_VEL);
+
+            VEL_X *= XY_FRICTION;
+            VEL_Y *= XY_FRICTION;
+            VEL_R *= ROT_FRICTION;
+
+            // println!("{}", VEL_R);
+
+            POS_X += VEL_X;
+            POS_Y += VEL_Y;
+            POS_R += VEL_R;
+
+
+
+            W1.swerve_rot = POS_R;
+            W2.swerve_rot = POS_R;
+            W3.swerve_rot = POS_R;
+            W4.swerve_rot = POS_R;
+
+
+            //Obtain joystick data and define the heading
+            let joyHeading = (get_heading(VEL_X, VEL_Y));
+            let heading = joyHeading + POS_R;
+            let speed = swerve_math::get_joystick_speed(VEL_X, VEL_Y);
+
+            //Define the steering vector components and the max vector length
+            let xr = VEL_R  * f32::cos(degrees_to_radians(45.)); // /2D normally
+            let yr = VEL_R  * f32::sin(degrees_to_radians(45.)); // /2D normally
+
+            //Calculate the vectors for all wheels
+            let x = get_heading_x(heading);
+            let y = get_heading_y(heading);
+
+
+            W1.vec.set((x*speed + xr) * VECTOR_SCALE, (y*speed + yr) * VECTOR_SCALE);
+            W2.vec.set((x*speed + xr) * VECTOR_SCALE, (y*speed - yr) * VECTOR_SCALE);
+            W3.vec.set((x*speed - xr) * VECTOR_SCALE, (y*speed - yr) * VECTOR_SCALE);
+            W4.vec.set((x*speed - xr) * VECTOR_SCALE, (y*speed + yr) * VECTOR_SCALE);
+
+            println!("{}", W1.vec.x);
+
+            W1.vec.rotate(POS_R);
+            W2.vec.rotate(POS_R);
+            W3.vec.rotate(POS_R);
+            W4.vec.rotate(POS_R);
+
+            thread::sleep(Duration::from_millis(10));
+            // TIME += 1;
+        }
+    }
+
+    let thread = tokio::spawn(unsafe { update() });
+
+    while let Some(event) = window.next() {
+        window.draw_2d(&event, |c, g, _d| unsafe {
+            clear(color::BLACK, g);
+
+
+            line(color::RED, 1., [
+                SCREEN_SQUARE_SIZE as f64/2.,
+                SCREEN_SQUARE_SIZE as f64/2.,
+                (f32::cos(degrees_to_radians(TARGET_R as f32)) * SWERVE_CENTER_DISTANCE as f32 + (SCREEN_SQUARE_SIZE/2) as f32) as f64,
+                (f32::sin(degrees_to_radians(TARGET_R as f32)) * SWERVE_CENTER_DISTANCE as f32 + (SCREEN_SQUARE_SIZE/2) as f32) as f64,
+            ], c.transform, g);
+
+
+            draw_horizontal_line(POS_Y, &c, g);
+            draw_horizontal_line(POS_Y + SCREEN_SQUARE_SIZE as f32 /2., &c, g);
+            draw_vertical_line(POS_X, &c, g);
+            draw_vertical_line(POS_X + SCREEN_SQUARE_SIZE as f32 /2., &c, g);
+
+            // if ROBO_RELATIVE {
+            SwerveModule::draw(&mut W1, &c, g,
+                               ((f32::cos(degrees_to_radians(POS_R - 45.)) * SWERVE_CENTER_DISTANCE as f32) + (SCREEN_SQUARE_SIZE/2) as f32) as u32,
+                               ((f32::sin(degrees_to_radians(POS_R - 45.)) * SWERVE_CENTER_DISTANCE as f32) + (SCREEN_SQUARE_SIZE/2) as f32) as u32);
+
+            SwerveModule::draw(&mut W2, &c, g,
+                               ((f32::cos(degrees_to_radians(POS_R - 135.)) * SWERVE_CENTER_DISTANCE as f32) + (SCREEN_SQUARE_SIZE/2) as f32) as u32,
+                               ((f32::sin(degrees_to_radians(POS_R - 135.)) * SWERVE_CENTER_DISTANCE as f32) + (SCREEN_SQUARE_SIZE/2) as f32) as u32);
+
+            SwerveModule::draw(&mut W3, &c, g,
+                               ((f32::cos(degrees_to_radians(POS_R + 135.)) * SWERVE_CENTER_DISTANCE as f32) + (SCREEN_SQUARE_SIZE/2) as f32) as u32,
+                               ((f32::sin(degrees_to_radians(POS_R + 135.)) * SWERVE_CENTER_DISTANCE as f32) + (SCREEN_SQUARE_SIZE/2) as f32) as u32);
+
+            SwerveModule::draw(&mut W4, &c, g,
+                               ((f32::cos(degrees_to_radians(POS_R + 45.)) * SWERVE_CENTER_DISTANCE as f32) + (SCREEN_SQUARE_SIZE/2) as f32) as u32,
+                               ((f32::sin(degrees_to_radians(POS_R + 45.)) * SWERVE_CENTER_DISTANCE as f32) + (SCREEN_SQUARE_SIZE/2) as f32) as u32);
+
+
+            draw_text(&c, g, &mut glyphs, color::WHITE, 0., 15., format!("POS: [{},{}] VEL_XY: [{},{}]", POS_X, POS_Y, VEL_X, VEL_Y).as_str());
+            draw_text(&c, g, &mut glyphs, color::WHITE, 0., 30., format!("ROT: {} VEL_ROT: {}", POS_R, VEL_R).as_str());
+
+            Rectangle::new_border(color::WHITE, 1.0).draw(square(0.0, 0.0, SCREEN_SQUARE_SIZE as Scalar), &c.draw_state, c.transform, g);
+
+            glyphs.factory.encoder.flush(_d);
         });
+
+        unsafe {
+            if let Some(Button::Keyboard(key)) = event.press_args() {
+                if key.code() == 119 {
+                    BUTTONS.key_w = true;
+                } else if (key.code() == 97) {
+                    BUTTONS.key_a = true;
+                } else if (key.code() == 115) {
+                    BUTTONS.key_s = true;
+                } else if (key.code() == 100) {
+                    BUTTONS.key_d = true;
+                } else if (key.code() == 113) {
+                    BUTTONS.key_q = true;
+                } else if (key.code() == 101) {
+                    BUTTONS.key_e = true;
+                } else if key.code() == 114 {
+                    // ROBO_RELATIVE = !ROBO_RELATIVE;
+                } else if key.code() == 120 {
+                    thread.abort();
+                    break;
                 }
             }
+
+            if let Some(Button::Keyboard(key)) = event.release_args() {
+                if key.code() == 119 {
+                    BUTTONS.key_w = false;
+                } else if (key.code() == 97) {
+                    BUTTONS.key_a = false;
+                } else if (key.code() == 115) {
+                    BUTTONS.key_s = false;
+                } else if (key.code() == 100) {
+                    BUTTONS.key_d = false;
+                } else if (key.code() == 113) {
+                    BUTTONS.key_q = false;
+                } else if (key.code() == 101) {
+                    BUTTONS.key_e = false;
+                }
+            }
+        }
+    }
+}
+
+pub fn draw_text(
+    ctx: &Context,
+    graphics: &mut G2d,
+    glyphs: &mut Glyphs,
+    color: Color,
+    x: f64,
+    y: f64,
+    text: &str,
+) {
+    text::Text::new_color(color, 15
+    )
+        .draw(
+            text,
+            glyphs,
+            &ctx.draw_state,
+            ctx.transform.trans(x,y),
+            graphics,
+        )
+        .unwrap();
+}

src/pid.rs

@@ -0,0 +1,44 @@
+pub(crate) struct PIDController {
+    pub kp: f64,
+    pub ki: f64,
+    pub kd: f64,
+    setpoint: f64,
+    integral: f64,
+    prev_error: f64,
+}
+
+impl PIDController {
+    pub(crate) fn new(kp: f64, ki: f64, kd: f64, setpoint: f64) -> Self {
+        PIDController {
+            kp,
+            ki,
+            kd,
+            setpoint,
+            integral: 0.0,
+            prev_error: 0.0,
+        }
+    }
+
+    pub fn update(&mut self, process_variable: f64, dt: f64) -> f64 {
+        let error = self.setpoint - process_variable;
+
+        // Proportional term
+        let p = self.kp * error;
+
+        // Integral term
+        self.integral += error * dt;
+        let i = self.ki * self.integral;
+
+        // Derivative term
+        let d = self.kd * (error - self.prev_error) / dt;
+
+        self.prev_error = error;
+
+        // Calculate and return the control output
+        p + i + d
+    }
+
+    pub fn set_setpoint(&mut self, setpoint: f64) {
+        self.setpoint = setpoint;
+    }
+}

src/swerve_math.rs

@@ -3,19 +3,25 @@ use std::f32::consts::PI;
 //https://github.com/Pantherbotics/SwerveSim/blob/master/src/main/java/com/pantherbotics/swervesim/util/Vector2d.java
 #[derive(Clone, Copy)]
 pub struct Vector2d {
-    x: f32,
+    pub(crate) x: f32,
-    y: f32
+    pub(crate) y: f32
 }
 impl Vector2d {
-    pub fn create(mut self, x:f32, y:f32) {
+    pub const fn create(mut self, x:f32, y:f32) {
         self.x = x;
         self.y = y;
     }
-    pub fn rotate(mut self, angle: f32) {
+
-        let cos_a:f32 = f32::cos(angle * (PI / 180.0));
+    pub fn set(&mut self, x:f32, y:f32) {
-        let sin_a:f32 = f32::sin(angle * (PI / 180.0));
+        self.x = x;
-        self.x = self.x * cos_a - self.y * sin_a;
+        self.y = y;
-        self.y = self.x * sin_a + self.y * cos_a;
+    }
+
+    pub fn rotate(&mut self, angle: f32) {
+        let mag = f32::sqrt(self.x*self.x+self.y*self.y);
+        let ang = ((f32::atan2(self.y, self.x) + degrees_to_radians(angle)));
+        self.x = f32::cos(ang)*mag;
+        self.y = f32::sin(ang)*mag;
     }
 
     /**
@@ -24,7 +30,7 @@ impl Vector2d {
      * @param vec Vector with which to perform dot product.
      * @return Dot product of this vector with argument.
      */
-    pub fn dot(mut self, vec: Vector2d) -> f32 {
+    pub fn dot(&mut self, vec: Vector2d) -> f32 {
         return self.x * vec.x + self.y * vec.y;
     }
 
@@ -104,7 +110,7 @@ pub fn get_heading_x(mut angle:f32) -> f32 {
  * The angles are in degrees from getHeading()
  * @param angle The angle (from getHeading()) to get the Y value for
  */
-pub fn getHeadingY(mut angle:f32) -> f32 {
+pub fn get_heading_y(mut angle:f32) -> f32 {
     //Ensure values are [0, 360)
     while angle > 360. { angle -= 360.; }
     while angle < 0. { angle += 360.; }
@@ -141,8 +147,8 @@ pub fn approach_zero(value:f32, shift:f32) -> f32 {
  * @param X the X of a coordinate [-1, 1]
  * @param Y the Y of a coordinate [-1, 1]
  */
-pub fn get_joystick_speed(Y:f32, X:f32) -> f32 {
+pub fn get_joystick_speed(y:f32, x:f32) -> f32 {
-    let mut v: Vector2d = Vector2d {x:X, y:Y};
+    let mut v: Vector2d = Vector2d {x:x, y:y};
 
     let angle = f32::atan2(v.x, v.y);
     let max_magnitude = if f32::abs(v.x) > f32::abs(v.y) {1. / f32::sin(angle)} else {1. / f32::cos(angle)};

src/swervemodule.rs

@@ -1,46 +1,70 @@
-use piston_window::{color, Context, Ellipse, G2d, rectangle};
+use piston_window::{color, Context, Ellipse, G2d, Line, line, rectangle};
+use piston_window::math::Scalar;
 use crate::swerve_math;
+use crate::swerve_math::{get_heading, get_heading_x, get_heading_y, Vector2d};
 
+#[derive(Clone, Copy)]
 pub struct SwerveModule {
-    id:i32,
+    pub(crate) id:i32,
-    w:i32,
+    pub(crate) swerve_rot: f32,
-    h: i32,
+    pub(crate) vec: Vector2d,
-    swerve_rot: f32
+
+    // pub(crate) vec_x: f32,
+    // pub(crate) vec_y: f32,
+    // pub(crate) vec_speed: f32,
+    // pub(crate) vec_angle: f32,
 }
 impl SwerveModule {
-
+    pub const fn new(id: i32) -> Self {
-    /**
+        SwerveModule {
-     * Draws the wheel vector given wheel data and screen data.
+            id:id,
-     * @param g the Graphics2D object to draw with
+            swerve_rot:0.,
-     * @param angle the angle of the wheel, in degrees
+            vec:Vector2d {x:0.,y:0.}
-     * @param speed the speed of the wheel [-1, 1]
+        }
-     * @param font the font size currently in use
+    }
-     * @param startFactor the factor [0, 1] to start the vector at, when scaling towards the center
+    pub fn draw(&mut self, c: &Context, mut g: &mut G2d, screen_x:u32, screen_y:u32) {
-     * @param width the width of the swerve box (in pixels)
-     * @param height the height of the swerve box (in pixels)
-     * @param vecScale the scalar for the wheel vector
-     */
-    pub fn draw(c: &Context, mut g: &mut G2d,
-        angle:f32, speed:f32, font:i32, startFactor:f32, width:f32, height:f32, vecScale:f32) {
     // pub fn draw(c: &Context, mut g: &mut G2d) {
         Ellipse::new_border(color::WHITE, 1.0).draw(
-            rectangle::centered_square(10.0, 10.0, 50.0),
+            rectangle::centered_square(screen_x as Scalar, screen_y as Scalar, 50.0),
             &c.draw_state,
             c.transform,
             g,
         );
+
+        // let vec_x = get_heading_x(self.vec_angle) * 50.;
+        // let vec_y = get_heading_y(self.vec_angle * 50.);
+
+        line(color::WHITE, 1.0,
+             [screen_x as f64,
+                 screen_y  as f64,
+                 (screen_x as f32 + self.vec.x) as f64,
+                 (screen_y as f32 + self.vec.y) as f64],
+             c.transform, g);
     }
 
-    fn get_swerve_corner_x(id: i8) -> f32 {
+    fn get_swerve_corner_x(&mut self, id: i8) -> f32 {
-        if (id == 1) {
+        if id == 1 {
-            return swerve_math::get_heading_x(-45 + swerve_rot);
+            return swerve_math::get_heading_x(-45. + self.swerve_rot);
-        }else if (id == 2) {
+        }else if id == 2 {
-            return swerve_math::get_heading_x(45 + swerve_rot);
+            return swerve_math::get_heading_x(45. + self.swerve_rot);
-        }else if (id == 3) {
+        }else if id == 3 {
-            return swerve_math::get_heading_x(135 + swerve_rot);
+            return swerve_math::get_heading_x(135. + self.swerve_rot);
-        }else if (id == 4) {
+        }else if id == 4 {
-            return swerve_math::get_heading_x(-135 + swerve_rot);
+            return swerve_math::get_heading_x(-135. + self.swerve_rot);
         }
-        return 0;
+        return 0.;
+    }
+
+    fn get_swerve_corner_y(&mut self, id: i8) -> f32 {
+        if id == 1 {
+            return swerve_math::get_heading_y(45. + self.swerve_rot);
+        }else if id == 2 {
+            return swerve_math::get_heading_y(45. + self.swerve_rot);
+        }else if id == 3 {
+            return swerve_math::get_heading_y(135. + self.swerve_rot);
+        }else if id == 4 {
+            return swerve_math::get_heading_y(-135. + self.swerve_rot);
+        }
+        return 0.;
     }
 }