mirror of
https://github.com/Astatin3/rust-swerve-sim.git
synced 2026-06-09 00:28:04 -06:00
Make the thing
This commit is contained in:
astatin3
+287
-21
@@ -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();
|
||||
|
||||
|
||||
fn draw_vertical_line(pos_x: f32, c: &Context, mut g: &mut G2d){
|
||||
let mut x = -pos_x % SCREEN_SQUARE_SIZE as f32;
|
||||
if x < 0. {x += SCREEN_SQUARE_SIZE as f32}
|
||||
|
||||
line(color::GRAY, 1., [
|
||||
x as f64,
|
||||
0.,
|
||||
x as f64,
|
||||
SCREEN_SQUARE_SIZE as f64,
|
||||
], c.transform, g);
|
||||
}
|
||||
|
||||
fn draw_horizontal_line(pos_y: f32, c: &Context, mut g: &mut G2d){
|
||||
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);
|
||||
}
|
||||
|
||||
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.
|
||||
);
|
||||
|
||||
|
||||
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() {
|
||||
if let Some(Button::Keyboard(key)) = event.press_args() {
|
||||
println!("Key pressed: {:?}", key);
|
||||
if key.code() == 113 {break;}
|
||||
}
|
||||
|
||||
if let Some(Button::Mouse(key)) = event.press_args() {
|
||||
println!("Key pressed: {:?}", key);
|
||||
}
|
||||
|
||||
window.draw_2d(&event, |c, g, _d| {
|
||||
window.draw_2d(&event, |c, g, _d| unsafe {
|
||||
clear(color::BLACK, g);
|
||||
|
||||
// Ellipse::new_border(color::WHITE, 1.0).draw(
|
||||
// rectangle::centered_square(10.0, 10.0, 50.0),
|
||||
// &c.draw_state,
|
||||
// c.transform,
|
||||
// g,
|
||||
// );
|
||||
|
||||
swervemodule::SwerveModule::draw(&c,);
|
||||
|
||||
Rectangle::new_border(color::WHITE, 1.0).draw(square(0.0, 0.0, 512.0), &c.draw_state, 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,
|
||||
], 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();
|
||||
}
|
||||
+44
@@ -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;
|
||||
}
|
||||
}
|
||||
+18
-12
@@ -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,
|
||||
y: f32
|
||||
pub(crate) x: 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));
|
||||
let sin_a:f32 = f32::sin(angle * (PI / 180.0));
|
||||
self.x = self.x * cos_a - self.y * sin_a;
|
||||
self.y = self.x * sin_a + self.y * cos_a;
|
||||
|
||||
pub fn set(&mut self, x:f32, y:f32) {
|
||||
self.x = x;
|
||||
self.y = 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)));
|
||||
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 {
|
||||
let mut v: Vector2d = Vector2d {x:X, y:Y};
|
||||
pub fn get_joystick_speed(y:f32, x:f32) -> f32 {
|
||||
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)};
|
||||
|
||||
+54
-30
@@ -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,
|
||||
w:i32,
|
||||
h: i32,
|
||||
swerve_rot: f32
|
||||
pub(crate) id:i32,
|
||||
pub(crate) swerve_rot: f32,
|
||||
pub(crate) vec: Vector2d,
|
||||
|
||||
// pub(crate) vec_x: f32,
|
||||
// pub(crate) vec_y: f32,
|
||||
// pub(crate) vec_speed: f32,
|
||||
// pub(crate) vec_angle: f32,
|
||||
}
|
||||
impl SwerveModule {
|
||||
|
||||
/**
|
||||
* Draws the wheel vector given wheel data and screen data.
|
||||
* @param g the Graphics2D object to draw with
|
||||
* @param angle the angle of the wheel, in degrees
|
||||
* @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
|
||||
* @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 const fn new(id: i32) -> Self {
|
||||
SwerveModule {
|
||||
id:id,
|
||||
swerve_rot:0.,
|
||||
vec:Vector2d {x:0.,y:0.}
|
||||
}
|
||||
}
|
||||
pub fn draw(&mut self, c: &Context, mut g: &mut G2d, screen_x:u32, screen_y:u32) {
|
||||
// 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 {
|
||||
if (id == 1) {
|
||||
return swerve_math::get_heading_x(-45 + swerve_rot);
|
||||
}else if (id == 2) {
|
||||
return swerve_math::get_heading_x(45 + swerve_rot);
|
||||
}else if (id == 3) {
|
||||
return swerve_math::get_heading_x(135 + swerve_rot);
|
||||
}else if (id == 4) {
|
||||
return swerve_math::get_heading_x(-135 + swerve_rot);
|
||||
fn get_swerve_corner_x(&mut self, id: i8) -> f32 {
|
||||
if id == 1 {
|
||||
return swerve_math::get_heading_x(-45. + self.swerve_rot);
|
||||
}else if id == 2 {
|
||||
return swerve_math::get_heading_x(45. + self.swerve_rot);
|
||||
}else if id == 3 {
|
||||
return swerve_math::get_heading_x(135. + self.swerve_rot);
|
||||
}else if id == 4 {
|
||||
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.;
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user