From 9c38e57b6550115ea9488bfd134c0db79e84e84b Mon Sep 17 00:00:00 2001 From: astatin3 Date: Sat, 10 Aug 2024 01:11:11 -0600 Subject: [PATCH] Make the thing work using swerve drives to move the positon of thr robot --- src/main.rs | 136 +++++++++++++++++++++++++++----------------- src/swerve_math.rs | 20 +++++-- src/swervemodule.rs | 18 +++++- 3 files changed, 115 insertions(+), 59 deletions(-) diff --git a/src/main.rs b/src/main.rs index 7a8d055..2719890 100644 --- a/src/main.rs +++ b/src/main.rs @@ -4,13 +4,14 @@ mod pid; extern crate piston_window; +use std::f64::consts::TAU; 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::swerve_math::{degrees_to_radians, get_heading, get_heading_x, get_heading_y, radians_to_degrees, Vector2d}; use crate::swervemodule::SwerveModule; @@ -18,13 +19,14 @@ 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 ROT_ACCEL:f32 = 0.5; +static MAX_XY_VEL:f32 = 15.; static MAX_ROT_VEL:f32 = 32.; -static VECTOR_SCALE:f32 = 20.; +static VECTOR_SCALE:f32 = 2000.; static XY_FRICTION:f32 = 0.95; -static ROT_FRICTION:f32 = 0.998; +static ROT_FRICTION:f32 = 0.98; // static mut TIME: u64 = 0; @@ -57,9 +59,12 @@ static mut W4:SwerveModule = SwerveModule::new(4); - +static mut TARGET_VEL_X: f32 = 0.; +static mut TARGET_VEL_Y: f32 = 0.; static mut TARGET_R: f32 = 0.; -static mut TARGET_R_DELTA: f32 = 2.; +static TARGET_VEL_XY : f32 = 0.1; +// static mut TARGET_R: f32 = 0.; +static TARGET_R_DELTA: f32 = 4.; #[tokio::main] async fn main() { @@ -100,39 +105,29 @@ async fn main() { } async unsafe fn update() { - // let mut pid_x: pid::PIDController = pid::PIDController::new( - // 2., - // 0.1, - // 9., - // 0. - // ); - // - // let mut pid_y: pid::PIDController = pid::PIDController::new( - // 2., - // 0.1, - // 9., - // 0. - // ); - let mut pid_rot: pid::PIDController = pid::PIDController::new( - 5.0, - 0.1, - 100., + 0.05, + 0., + 0.2, 0. ); loop { if BUTTONS.key_w { - VEL_Y -= (XY_ACCEL); + TARGET_VEL_Y = -TARGET_VEL_XY; } else if BUTTONS.key_s { - VEL_Y += (XY_ACCEL); + TARGET_VEL_Y = TARGET_VEL_XY; + }else{ + TARGET_VEL_Y = 0.; } if BUTTONS.key_a { - VEL_X -= (XY_ACCEL); + TARGET_VEL_X = -TARGET_VEL_XY; } else if BUTTONS.key_d { - VEL_X += (XY_ACCEL); + TARGET_VEL_X = TARGET_VEL_XY; + }else{ + TARGET_VEL_X = 0.; } if BUTTONS.key_q { @@ -147,21 +142,8 @@ async fn main() { - 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; + // VEL_R = f32::clamp(VEL_R, -MAX_ROT_VEL, MAX_ROT_VEL); W1.swerve_rot = POS_R; @@ -171,31 +153,72 @@ async fn main() { //Obtain joystick data and define the heading - let joyHeading = (get_heading(VEL_X, VEL_Y)); + let joyHeading = get_heading(TARGET_VEL_X, TARGET_VEL_Y); let heading = joyHeading + POS_R; - let speed = swerve_math::get_joystick_speed(VEL_X, VEL_Y); + let speed = swerve_math::get_joystick_speed(TARGET_VEL_X, TARGET_VEL_Y); + + let target_vel_r = pid_rot.update(POS_R as f64, 1.) as f32; //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 + let xr = target_vel_r * f32::cos(degrees_to_radians(45.)); // /2D normally + let yr = target_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.set((x*speed + xr), (y*speed + yr)); + W2.vec.set((x*speed + xr), (y*speed - yr)); + W3.vec.set((x*speed - xr), (y*speed - yr)); + W4.vec.set((x*speed - xr), (y*speed + yr)); W1.vec.rotate(POS_R); W2.vec.rotate(POS_R); W3.vec.rotate(POS_R); W4.vec.rotate(POS_R); + let DEGREES_PER_U:f32 = radians_to_degrees(1. / SWERVE_CENTER_DISTANCE as f32); + + // println!("{}, {}, {}, {}", + // f32::cos(degrees_to_radians(W1.get_rotation())), + // f32::cos(degrees_to_radians(W2.get_rotation())), + // f32::cos(degrees_to_radians(W3.get_rotation())), + // f32::cos(degrees_to_radians(W4.get_rotation()))); + + // VEL_R = 1.2; + + // println!("{}", W1.get_rotation()); + + VEL_R += (f32::cos(degrees_to_radians(W1.get_rotation())) * W1.vec.magnitude() * DEGREES_PER_U + + f32::cos(degrees_to_radians(W2.get_rotation())) * W2.vec.magnitude() * DEGREES_PER_U + + f32::cos(degrees_to_radians(W3.get_rotation())) * W3.vec.magnitude() * DEGREES_PER_U + + f32::cos(degrees_to_radians(W4.get_rotation())) * W4.vec.magnitude() * DEGREES_PER_U) * ROT_ACCEL; + + + println!("{}", VEL_R); + + let mut module_sum: Vector2d = Vector2d::create(0.,0.); + module_sum.add(W1.vec); + module_sum.add(W2.vec); + module_sum.add(W3.vec); + module_sum.add(W4.vec); + + VEL_X += module_sum.x; + VEL_Y += module_sum.y; + + + + VEL_X = VEL_X.clamp(-MAX_XY_VEL, MAX_XY_VEL) * XY_FRICTION; + VEL_Y = VEL_Y.clamp(-MAX_XY_VEL, MAX_XY_VEL) * XY_FRICTION; + VEL_R = VEL_R.clamp(-MAX_ROT_VEL, MAX_ROT_VEL) * ROT_FRICTION; + + // println!("{}", VEL_R); + + POS_X += VEL_X; + POS_Y += VEL_Y; + POS_R += VEL_R; + thread::sleep(Duration::from_millis(10)); // TIME += 1; } @@ -207,15 +230,22 @@ async fn main() { window.draw_2d(&event, |c, g, _d| unsafe { clear(color::BLACK, g); + line(color::BLUE, 1., [ + SCREEN_SQUARE_SIZE as f64/2., + SCREEN_SQUARE_SIZE as f64/2., + (f32::cos(degrees_to_radians(POS_R)) * SWERVE_CENTER_DISTANCE as f32 + (SCREEN_SQUARE_SIZE/2) as f32) as f64, + (f32::sin(degrees_to_radians(POS_R)) * SWERVE_CENTER_DISTANCE as f32 + (SCREEN_SQUARE_SIZE/2) as f32) as f64, + ], c.transform, 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, + (f32::cos(degrees_to_radians(TARGET_R)) * SWERVE_CENTER_DISTANCE as f32 + (SCREEN_SQUARE_SIZE/2) as f32) as f64, + (f32::sin(degrees_to_radians(TARGET_R)) * 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); diff --git a/src/swerve_math.rs b/src/swerve_math.rs index 0f48edf..56d779b 100644 --- a/src/swerve_math.rs +++ b/src/swerve_math.rs @@ -1,4 +1,5 @@ use std::f32::consts::PI; +use crate::swervemodule::SwerveModule; //https://github.com/Pantherbotics/SwerveSim/blob/master/src/main/java/com/pantherbotics/swervesim/util/Vector2d.java #[derive(Clone, Copy)] @@ -7,9 +8,11 @@ pub struct Vector2d { pub(crate) y: f32 } impl Vector2d { - pub const fn create(mut self, x:f32, y:f32) { - self.x = x; - self.y = y; + pub const fn create(x:f32, y:f32) -> Vector2d { + Vector2d { + x, + y + } } pub fn set(&mut self, x:f32, y:f32) { @@ -17,9 +20,14 @@ impl Vector2d { self.y = y; } + pub fn add(&mut self, other: Vector2d) { + self.x += other.x; + self.y += other.y; + } + 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))); + let ang = f32::atan2(self.y, self.x) + degrees_to_radians(angle); self.x = f32::cos(ang)*mag; self.y = f32::sin(ang)*mag; } @@ -53,6 +61,10 @@ impl Vector2d { let mag = vec.magnitude(); return self.dot(vec) / mag; } + + pub fn get_ang(self) -> f32 { + return radians_to_degrees(f32::atan2(self.y, self.x)); + } } pub fn degrees_to_radians(degrees: f32) -> f32{ diff --git a/src/swervemodule.rs b/src/swervemodule.rs index fa2a367..c3c0d06 100644 --- a/src/swervemodule.rs +++ b/src/swervemodule.rs @@ -1,3 +1,4 @@ +use std::process::id; use piston_window::{color, Context, Ellipse, G2d, Line, line, rectangle}; use piston_window::math::Scalar; use crate::swerve_math; @@ -37,11 +38,24 @@ impl SwerveModule { 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], + (screen_x as f32 + self.vec.x*500.) as f64, + (screen_y as f32 + self.vec.y*500.) as f64], c.transform, g); } + pub fn get_rotation(&mut self) -> f32 { + if self.id == 1 { + return ((-45. + self.vec.get_ang() - self.swerve_rot) % 360.); + }else if self.id == 2 { + return ((45. + self.vec.get_ang() - self.swerve_rot) % 360.); + }else if self.id == 3 { + return ((135. + self.vec.get_ang() - self.swerve_rot) % 360.); + }else if self.id == 4 { + return ((-135. + self.vec.get_ang() - self.swerve_rot) % 360.); + } + return 0.; + } + fn get_swerve_corner_x(&mut self, id: i8) -> f32 { if id == 1 { return swerve_math::get_heading_x(-45. + self.swerve_rot);