From aeb11dfbaafa499a6eaf165b8678098f1afac29c Mon Sep 17 00:00:00 2001 From: TheSnowHatHero Date: Sat, 1 Feb 2025 13:02:35 -0600 Subject: [PATCH 1/5] drivetrain works now --- src/subsystems/drivetrain.rs | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/subsystems/drivetrain.rs b/src/subsystems/drivetrain.rs index 42c30ac..ee6ff7e 100644 --- a/src/subsystems/drivetrain.rs +++ b/src/subsystems/drivetrain.rs @@ -224,12 +224,12 @@ impl Drivetrain { } pub fn set_speeds(&mut self, fwd: f64, str: f64, rot: f64) { - //println!("ODO X: {}", self.odometry.position.x); + println!("ODO XY: {}, {}", self.odometry.position.x, self.odometry.position.y); let mut transform = Vector2::new(-str, fwd); transform = - Rotation2::new(-self.get_offset().get::() + std::f64::consts::PI) * transform; + Rotation2::new(self.get_offset().get::()) * transform; - let wheel_speeds = self.kinematics.calculate(transform, rot); + let wheel_speeds = self.kinematics.calculate(transform, -rot); let measured = self.get_speeds(); @@ -301,9 +301,9 @@ impl Drivetrain { pub fn get_angle(&self) -> Angle { if alliance_station().red() { - Angle::new::(-self.pigeon.get_rotation().z + 180.) + Angle::new::(-self.pigeon.get_rotation().z + 180.) } else { - Angle::new::(-self.pigeon.get_rotation().z) + Angle::new::(-self.pigeon.get_rotation().z) } } From e3f5e46c37bd5e6c38fb4668a0ee7343248dcb85 Mon Sep 17 00:00:00 2001 From: TheSnowHatHero Date: Sat, 1 Feb 2025 18:32:04 -0600 Subject: [PATCH 2/5] Elevator trapezoidal & tuning --- src/constants.rs | 6 +++-- src/lib.rs | 29 +++++++++++++-------- src/subsystems/elevator.rs | 52 +++++++++++++++++++++++++++++++------- 3 files changed, 65 insertions(+), 22 deletions(-) diff --git a/src/constants.rs b/src/constants.rs index ea6a72b..f1b3c88 100644 --- a/src/constants.rs +++ b/src/constants.rs @@ -66,7 +66,9 @@ pub mod drivetrain { } pub mod elevator { pub const BOTTOM: f64 = 0.0; - pub const L2: f64 = 0.0; - pub const L3: f64 = 0.0; + pub const L2: f64 = 1.; + pub const L3: f64 = 15.75; pub const L4: f64 = 39.7; + pub const ELEVATOR_TRAPEZOID_DT_MS: u64 = 50; + pub const POSITION_TOLERANCE: f64 = 0.25; } diff --git a/src/lib.rs b/src/lib.rs index 80ffd54..108b894 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -5,12 +5,10 @@ pub mod subsystems; pub mod swerve; use std::cell::RefCell; - +use std::cmp::PartialEq; use crate::auto::Auto; use crate::container::control_drivetrain; -use crate::subsystems::{ - Climber, Drivetrain, DrivetrainControlState, Elevator, Indexer, LineupSide, -}; +use crate::subsystems::{Climber, Drivetrain, DrivetrainControlState, Elevator, ElevatorPosition, Indexer, LineupSide}; use frcrs::input::Joystick; use frcrs::networktables::NetworkTable; use frcrs::telemetry::Telemetry; @@ -19,6 +17,7 @@ use std::ops::Deref; use std::rc::Rc; use std::sync::Arc; use tokio::task::spawn_local; +use uom::si::angle::degree; #[derive(Clone)] pub struct Controllers { @@ -164,22 +163,30 @@ impl Robot for Ferris { control_drivetrain(&mut drivetrain, &mut self.controllers, drivetrain_state).await; } } - - if let Ok(elevator) = self.elevator.try_borrow_mut() { + if let Ok(mut elevator) = self.elevator.try_borrow_mut() { + if self.controllers.operator.get(14){ + elevator.set_target(ElevatorPosition::L2); + } else if self.controllers.operator.get(15){ + elevator.set_target(ElevatorPosition::L3); + } else if self.controllers.operator.get(16) { + elevator.set_target(ElevatorPosition::L4); + } if self.controllers.operator.get(3) { - elevator.set_speed(0.5); + elevator.set_speed(0.1); } else if self.controllers.operator.get(4) { - elevator.set_speed(-0.5); + elevator.set_speed(-0.10); + } else if self.controllers.operator.get(1) { + elevator.run_to_target_trapezoid(); } else { elevator.set_speed(0.0); } } if let Ok(indexer) = self.indexer.try_borrow_mut() { - if self.controllers.operator.get(2) { + if self.controllers.left_drive.get(2) { indexer.set_speed(0.3); - } else if self.controllers.operator.get(1) { - indexer.set_speed(-0.1); + } else if self.controllers.left_drive.get(1) { + indexer.set_speed(-0.5); } else { indexer.set_speed(0.0); } diff --git a/src/subsystems/elevator.rs b/src/subsystems/elevator.rs index 9010e22..d5048f9 100644 --- a/src/subsystems/elevator.rs +++ b/src/subsystems/elevator.rs @@ -1,17 +1,34 @@ +use std::fmt::Display; +use std::time::Duration; use crate::constants::{elevator, robotmap}; use frcrs::ctre::{ControlMode, Talon}; +use tokio::time::sleep; pub struct Elevator { left: Talon, right: Talon, + target: ElevatorPosition } +#[derive(Copy, Clone, Debug)] pub enum ElevatorPosition { Bottom, L2, L3, L4, } +impl PartialEq for ElevatorPosition { + fn eq(&self, other: &Self) -> bool { + match (self, other) { + (ElevatorPosition::Bottom, ElevatorPosition::Bottom) => true, + (ElevatorPosition::L2, ElevatorPosition::L2) => true, + (ElevatorPosition::L3, ElevatorPosition::L3) => true, + (ElevatorPosition::L4, ElevatorPosition::L4) => true, + _ => false, + } + } +} + impl Default for Elevator { fn default() -> Self { @@ -22,24 +39,41 @@ impl Default for Elevator { impl Elevator { pub fn new() -> Self { let left = Talon::new(robotmap::elevator::LEFT, Some("can0".to_string())); - let right = Talon::new(robotmap::elevator::RIGHT, Some("can0".to_string())); + let right = Talon::new(robotmap::elevator::RIGHT, Some("can0".to_string())); //should be inverted (clockwise positive) in config - right.follow(&left, true); + //right.follow(&left, true); - Self { left, right } + Self { left, right, target: ElevatorPosition::Bottom } + } + pub fn set_target(&mut self, target: ElevatorPosition) { + self.target = target; } + pub fn get_target(&self) -> ElevatorPosition {self.target} - pub fn set_position(&self, position: ElevatorPosition) { - match position { - ElevatorPosition::Bottom => self.left.set(ControlMode::Position, elevator::BOTTOM), - ElevatorPosition::L2 => self.left.set(ControlMode::Position, elevator::L2), - ElevatorPosition::L3 => self.left.set(ControlMode::Position, elevator::L3), - ElevatorPosition::L4 => self.left.set(ControlMode::Position, elevator::L4), + pub fn run_to_target_trapezoid(&mut self) { + let target_position = match self.get_target(){ + ElevatorPosition::Bottom => {elevator::BOTTOM}, + ElevatorPosition::L2 => {elevator::L2}, + ElevatorPosition::L3 => {elevator::L3}, + ElevatorPosition::L4 => {elevator::L4}, + }; + self.right.set(ControlMode::MotionMagic, target_position); + self.left.set(ControlMode::MotionMagic, target_position); + /* + while (self.right.get_position() - target_position).abs() < elevator::POSITION_TOLERANCE { + self.right.set(ControlMode::MotionMagic, target_position); + self.left.set(ControlMode::MotionMagic, target_position); } + */ + } + pub fn set_position_rotations(&mut self, position: f64){ + self.left.set(ControlMode::Position, position); + self.right.set(ControlMode::Position, position); } pub fn set_speed(&self, speed: f64) { self.left.set(ControlMode::Percent, speed); + self.right.set(ControlMode::Percent, speed); //is inverted in config } pub fn stop(&self) { From 22ede4e7fe4acc4f120b7ea092d03ecfacd91bfa Mon Sep 17 00:00:00 2001 From: TheSnowHatHero Date: Sat, 1 Feb 2025 19:49:42 -0600 Subject: [PATCH 3/5] added comments to robot code from the last two days --- src/constants.rs | 14 +++++++------- src/lib.rs | 17 +++++++++++------ src/subsystems/elevator.rs | 19 +++++++++++++++---- 3 files changed, 33 insertions(+), 17 deletions(-) diff --git a/src/constants.rs b/src/constants.rs index f1b3c88..4d73e5b 100644 --- a/src/constants.rs +++ b/src/constants.rs @@ -65,10 +65,10 @@ pub mod drivetrain { pub const PODIUM_SHOT_ANGLE: f64 = 34.34; // degrees } pub mod elevator { - pub const BOTTOM: f64 = 0.0; - pub const L2: f64 = 1.; - pub const L3: f64 = 15.75; - pub const L4: f64 = 39.7; - pub const ELEVATOR_TRAPEZOID_DT_MS: u64 = 50; - pub const POSITION_TOLERANCE: f64 = 0.25; -} + pub const BOTTOM: f64 = 0.0; // unit is rotations + pub const L2: f64 = 1.; // unit is rotations + pub const L3: f64 = 15.75; // unit is rotations + pub const L4: f64 = 39.7; // unit is rotations + pub const ELEVATOR_TRAPEZOID_DT_MS: u64 = 50; // Currently unused TODO: update this comment when elevator trapezoidal move is async + pub const POSITION_TOLERANCE: f64 = 0.25; // Currently unused TODO: update this comment when elevator trapezoidal move is async +} \ No newline at end of file diff --git a/src/lib.rs b/src/lib.rs index 108b894..115ac5d 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -163,7 +163,10 @@ impl Robot for Ferris { control_drivetrain(&mut drivetrain, &mut self.controllers, drivetrain_state).await; } } + + if let Ok(mut elevator) = self.elevator.try_borrow_mut() { + // Setting the target position if self.controllers.operator.get(14){ elevator.set_target(ElevatorPosition::L2); } else if self.controllers.operator.get(15){ @@ -171,11 +174,13 @@ impl Robot for Ferris { } else if self.controllers.operator.get(16) { elevator.set_target(ElevatorPosition::L4); } - if self.controllers.operator.get(3) { + + // Setting the actual elevator operation + if self.controllers.operator.get(3) { // Manual up elevator.set_speed(0.1); - } else if self.controllers.operator.get(4) { - elevator.set_speed(-0.10); - } else if self.controllers.operator.get(1) { + } else if self.controllers.operator.get(4) { // Manual down + elevator.set_speed(-0.1); + } else if self.controllers.operator.get(1) { // Trapezoidal to stored target elevator.run_to_target_trapezoid(); } else { elevator.set_speed(0.0); @@ -183,9 +188,9 @@ impl Robot for Ferris { } if let Ok(indexer) = self.indexer.try_borrow_mut() { - if self.controllers.left_drive.get(2) { + if self.controllers.left_drive.get(2) { // Out the front indexer.set_speed(0.3); - } else if self.controllers.left_drive.get(1) { + } else if self.controllers.left_drive.get(1) { // In, score out the left indexer.set_speed(-0.5); } else { indexer.set_speed(0.0); diff --git a/src/subsystems/elevator.rs b/src/subsystems/elevator.rs index d5048f9..d922b33 100644 --- a/src/subsystems/elevator.rs +++ b/src/subsystems/elevator.rs @@ -45,20 +45,35 @@ impl Elevator { Self { left, right, target: ElevatorPosition::Bottom } } + /// Set the stored goal position. + /// TODO: update the below lines of this comment once controls scheme is figured out. + /// That position is referenced by run_to_target_trapezoid. + /// Not much benefit over just passing a target to that function yet but I have plans. pub fn set_target(&mut self, target: ElevatorPosition) { self.target = target; } + /// Get the stored goal position. + /// For stuff like LEDs. + /// Indexer used to reference this to get scoring speed, might need to go back to that once we have real bumpers pub fn get_target(&self) -> ElevatorPosition {self.target} + /// Runs a trapezoidal (motion magic) profile on the elevator krakens to move the elevator to its stored target position. + /// Most of the interesting stuff for this is in the elevator krakens' configurations (set in pheonix tuner). + /// Those configuration files have been saved to Documents on the driver station laptop. pub fn run_to_target_trapezoid(&mut self) { + //load position to run to in rotations from constants.rs let target_position = match self.get_target(){ ElevatorPosition::Bottom => {elevator::BOTTOM}, ElevatorPosition::L2 => {elevator::L2}, ElevatorPosition::L3 => {elevator::L3}, ElevatorPosition::L4 => {elevator::L4}, }; + + // current implementation is to just set the control mode and call this function every frame self.right.set(ControlMode::MotionMagic, target_position); self.left.set(ControlMode::MotionMagic, target_position); + + // below is for when we eventually make this function async /* while (self.right.get_position() - target_position).abs() < elevator::POSITION_TOLERANCE { self.right.set(ControlMode::MotionMagic, target_position); @@ -66,10 +81,6 @@ impl Elevator { } */ } - pub fn set_position_rotations(&mut self, position: f64){ - self.left.set(ControlMode::Position, position); - self.right.set(ControlMode::Position, position); - } pub fn set_speed(&self, speed: f64) { self.left.set(ControlMode::Percent, speed); From 33f8a6181e6f591ba0b9a70e656ea549368988c1 Mon Sep 17 00:00:00 2001 From: TheSnowHatHero Date: Sat, 1 Feb 2025 20:05:48 -0600 Subject: [PATCH 4/5] Made, used joystick map constants --- src/constants.rs | 23 +++++++++++++++++++++++ src/lib.rs | 29 ++++++++++++++--------------- 2 files changed, 37 insertions(+), 15 deletions(-) diff --git a/src/constants.rs b/src/constants.rs index 4d73e5b..c211f49 100644 --- a/src/constants.rs +++ b/src/constants.rs @@ -71,4 +71,27 @@ pub mod elevator { pub const L4: f64 = 39.7; // unit is rotations pub const ELEVATOR_TRAPEZOID_DT_MS: u64 = 50; // Currently unused TODO: update this comment when elevator trapezoidal move is async pub const POSITION_TOLERANCE: f64 = 0.25; // Currently unused TODO: update this comment when elevator trapezoidal move is async +} +pub mod joystick_map { + // Joystick IDs (set in driver station) + pub const RIGHT_DRIVE: i32 = 0; + pub const LEFT_DRIVE: i32 = 1; + pub const OPERATOR: i32 = 2; + + //Right drive + pub const LINEUP_LEFT: usize = 3; + pub const LINEUP_RIGHT: usize = 4; + + //Left drive + pub const INDEXER_IN: usize = 1; + pub const INDEXER_OUT: usize = 2; + + //Operator + pub const ELEVATOR_TRAPEZOID_TO_STORED_TARGET: usize = 1; + pub const ELEVATOR_UP_MANUAL: usize = 3; + pub const ELEVATOR_DOWN_MANUAL: usize = 4; + pub const CLIMB_FULL: usize = 8; + pub const SET_TARGET_L2: usize = 14; + pub const SET_TARGET_L3: usize = 15; + pub const SET_TARGET_L4: usize = 16; } \ No newline at end of file diff --git a/src/lib.rs b/src/lib.rs index 115ac5d..db6b3f9 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -17,7 +17,6 @@ use std::ops::Deref; use std::rc::Rc; use std::sync::Arc; use tokio::task::spawn_local; -use uom::si::angle::degree; #[derive(Clone)] pub struct Controllers { @@ -57,9 +56,9 @@ impl Ferris { Ferris { task_manager: TaskManager::new(), controllers: Controllers { - left_drive: Joystick::new(1), - right_drive: Joystick::new(0), - operator: Joystick::new(2), + left_drive: Joystick::new(constants::joystick_map::LEFT_DRIVE), + right_drive: Joystick::new(constants::joystick_map::RIGHT_DRIVE), + operator: Joystick::new(constants::joystick_map::OPERATOR), }, drivetrain: Rc::new(RefCell::new(Drivetrain::new())), elevator: Rc::new(RefCell::new(Elevator::new())), @@ -155,9 +154,9 @@ impl Robot for Ferris { //drivetrain.update_limelight().await; drivetrain.post_odo().await; - if self.controllers.right_drive.get(3) { + if self.controllers.right_drive.get(constants::joystick_map::LINEUP_LEFT) { drivetrain.lineup(LineupSide::Left).await; - } else if self.controllers.right_drive.get(4) { + } else if self.controllers.right_drive.get(constants::joystick_map::LINEUP_RIGHT) { drivetrain.lineup(LineupSide::Right).await; } else { control_drivetrain(&mut drivetrain, &mut self.controllers, drivetrain_state).await; @@ -167,20 +166,20 @@ impl Robot for Ferris { if let Ok(mut elevator) = self.elevator.try_borrow_mut() { // Setting the target position - if self.controllers.operator.get(14){ + if self.controllers.operator.get(constants::joystick_map::SET_TARGET_L2){ elevator.set_target(ElevatorPosition::L2); - } else if self.controllers.operator.get(15){ + } else if self.controllers.operator.get(constants::joystick_map::SET_TARGET_L3){ elevator.set_target(ElevatorPosition::L3); - } else if self.controllers.operator.get(16) { + } else if self.controllers.operator.get(constants::joystick_map::SET_TARGET_L4) { elevator.set_target(ElevatorPosition::L4); } // Setting the actual elevator operation - if self.controllers.operator.get(3) { // Manual up + if self.controllers.operator.get(constants::joystick_map::ELEVATOR_UP_MANUAL) { // Manual up elevator.set_speed(0.1); - } else if self.controllers.operator.get(4) { // Manual down + } else if self.controllers.operator.get(constants::joystick_map::ELEVATOR_DOWN_MANUAL) { // Manual down elevator.set_speed(-0.1); - } else if self.controllers.operator.get(1) { // Trapezoidal to stored target + } else if self.controllers.operator.get(constants::joystick_map::ELEVATOR_TRAPEZOID_TO_STORED_TARGET) { // Trapezoidal to stored target elevator.run_to_target_trapezoid(); } else { elevator.set_speed(0.0); @@ -188,9 +187,9 @@ impl Robot for Ferris { } if let Ok(indexer) = self.indexer.try_borrow_mut() { - if self.controllers.left_drive.get(2) { // Out the front + if self.controllers.left_drive.get(constants::joystick_map::INDEXER_OUT) { // Out the front indexer.set_speed(0.3); - } else if self.controllers.left_drive.get(1) { // In, score out the left + } else if self.controllers.left_drive.get(constants::joystick_map::INDEXER_IN) { // In, score out the left indexer.set_speed(-0.5); } else { indexer.set_speed(0.0); @@ -226,7 +225,7 @@ impl Robot for Ferris { } }; - if self.controllers.operator.get(8) { + if self.controllers.operator.get(constants::joystick_map::CLIMB_FULL) { self.task_manager.run_task(climb); } else { self.task_manager.run_task(cancel_climb); From 1cd14c0c3fd61012949f0d856c70c571afdda90a Mon Sep 17 00:00:00 2001 From: Nolan Peterson Date: Sat, 1 Feb 2025 19:17:33 -0700 Subject: [PATCH 5/5] fmt and change PartialEq to derive from impl --- src/constants.rs | 2 +- src/lib.rs | 80 +++++++++++++++++++++++++++++------- src/subsystems/drivetrain.rs | 8 ++-- src/subsystems/elevator.rs | 40 ++++++++---------- 4 files changed, 88 insertions(+), 42 deletions(-) diff --git a/src/constants.rs b/src/constants.rs index c211f49..82185f2 100644 --- a/src/constants.rs +++ b/src/constants.rs @@ -94,4 +94,4 @@ pub mod joystick_map { pub const SET_TARGET_L2: usize = 14; pub const SET_TARGET_L3: usize = 15; pub const SET_TARGET_L4: usize = 16; -} \ No newline at end of file +} diff --git a/src/lib.rs b/src/lib.rs index db6b3f9..c15e605 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -4,15 +4,17 @@ pub mod container; pub mod subsystems; pub mod swerve; -use std::cell::RefCell; -use std::cmp::PartialEq; use crate::auto::Auto; use crate::container::control_drivetrain; -use crate::subsystems::{Climber, Drivetrain, DrivetrainControlState, Elevator, ElevatorPosition, Indexer, LineupSide}; +use crate::subsystems::{ + Climber, Drivetrain, DrivetrainControlState, Elevator, ElevatorPosition, Indexer, LineupSide, +}; use frcrs::input::Joystick; use frcrs::networktables::NetworkTable; use frcrs::telemetry::Telemetry; use frcrs::{Robot, TaskManager}; +use std::cell::RefCell; +use std::cmp::PartialEq; use std::ops::Deref; use std::rc::Rc; use std::sync::Arc; @@ -154,32 +156,66 @@ impl Robot for Ferris { //drivetrain.update_limelight().await; drivetrain.post_odo().await; - if self.controllers.right_drive.get(constants::joystick_map::LINEUP_LEFT) { + if self + .controllers + .right_drive + .get(constants::joystick_map::LINEUP_LEFT) + { drivetrain.lineup(LineupSide::Left).await; - } else if self.controllers.right_drive.get(constants::joystick_map::LINEUP_RIGHT) { + } else if self + .controllers + .right_drive + .get(constants::joystick_map::LINEUP_RIGHT) + { drivetrain.lineup(LineupSide::Right).await; } else { control_drivetrain(&mut drivetrain, &mut self.controllers, drivetrain_state).await; } } - if let Ok(mut elevator) = self.elevator.try_borrow_mut() { // Setting the target position - if self.controllers.operator.get(constants::joystick_map::SET_TARGET_L2){ + if self + .controllers + .operator + .get(constants::joystick_map::SET_TARGET_L2) + { elevator.set_target(ElevatorPosition::L2); - } else if self.controllers.operator.get(constants::joystick_map::SET_TARGET_L3){ + } else if self + .controllers + .operator + .get(constants::joystick_map::SET_TARGET_L3) + { elevator.set_target(ElevatorPosition::L3); - } else if self.controllers.operator.get(constants::joystick_map::SET_TARGET_L4) { + } else if self + .controllers + .operator + .get(constants::joystick_map::SET_TARGET_L4) + { elevator.set_target(ElevatorPosition::L4); } // Setting the actual elevator operation - if self.controllers.operator.get(constants::joystick_map::ELEVATOR_UP_MANUAL) { // Manual up + if self + .controllers + .operator + .get(constants::joystick_map::ELEVATOR_UP_MANUAL) + { + // Manual up elevator.set_speed(0.1); - } else if self.controllers.operator.get(constants::joystick_map::ELEVATOR_DOWN_MANUAL) { // Manual down + } else if self + .controllers + .operator + .get(constants::joystick_map::ELEVATOR_DOWN_MANUAL) + { + // Manual down elevator.set_speed(-0.1); - } else if self.controllers.operator.get(constants::joystick_map::ELEVATOR_TRAPEZOID_TO_STORED_TARGET) { // Trapezoidal to stored target + } else if self + .controllers + .operator + .get(constants::joystick_map::ELEVATOR_TRAPEZOID_TO_STORED_TARGET) + { + // Trapezoidal to stored target elevator.run_to_target_trapezoid(); } else { elevator.set_speed(0.0); @@ -187,9 +223,19 @@ impl Robot for Ferris { } if let Ok(indexer) = self.indexer.try_borrow_mut() { - if self.controllers.left_drive.get(constants::joystick_map::INDEXER_OUT) { // Out the front + if self + .controllers + .left_drive + .get(constants::joystick_map::INDEXER_OUT) + { + // Out the front indexer.set_speed(0.3); - } else if self.controllers.left_drive.get(constants::joystick_map::INDEXER_IN) { // In, score out the left + } else if self + .controllers + .left_drive + .get(constants::joystick_map::INDEXER_IN) + { + // In, score out the left indexer.set_speed(-0.5); } else { indexer.set_speed(0.0); @@ -225,7 +271,11 @@ impl Robot for Ferris { } }; - if self.controllers.operator.get(constants::joystick_map::CLIMB_FULL) { + if self + .controllers + .operator + .get(constants::joystick_map::CLIMB_FULL) + { self.task_manager.run_task(climb); } else { self.task_manager.run_task(cancel_climb); diff --git a/src/subsystems/drivetrain.rs b/src/subsystems/drivetrain.rs index ee6ff7e..a5e17a0 100644 --- a/src/subsystems/drivetrain.rs +++ b/src/subsystems/drivetrain.rs @@ -224,10 +224,12 @@ impl Drivetrain { } pub fn set_speeds(&mut self, fwd: f64, str: f64, rot: f64) { - println!("ODO XY: {}, {}", self.odometry.position.x, self.odometry.position.y); + println!( + "ODO XY: {}, {}", + self.odometry.position.x, self.odometry.position.y + ); let mut transform = Vector2::new(-str, fwd); - transform = - Rotation2::new(self.get_offset().get::()) * transform; + transform = Rotation2::new(self.get_offset().get::()) * transform; let wheel_speeds = self.kinematics.calculate(transform, -rot); diff --git a/src/subsystems/elevator.rs b/src/subsystems/elevator.rs index d922b33..6e7ec0b 100644 --- a/src/subsystems/elevator.rs +++ b/src/subsystems/elevator.rs @@ -1,34 +1,22 @@ -use std::fmt::Display; -use std::time::Duration; use crate::constants::{elevator, robotmap}; use frcrs::ctre::{ControlMode, Talon}; +use std::fmt::Display; +use std::time::Duration; use tokio::time::sleep; pub struct Elevator { left: Talon, right: Talon, - target: ElevatorPosition + target: ElevatorPosition, } -#[derive(Copy, Clone, Debug)] +#[derive(Copy, Clone, Debug, PartialEq)] pub enum ElevatorPosition { Bottom, L2, L3, L4, } -impl PartialEq for ElevatorPosition { - fn eq(&self, other: &Self) -> bool { - match (self, other) { - (ElevatorPosition::Bottom, ElevatorPosition::Bottom) => true, - (ElevatorPosition::L2, ElevatorPosition::L2) => true, - (ElevatorPosition::L3, ElevatorPosition::L3) => true, - (ElevatorPosition::L4, ElevatorPosition::L4) => true, - _ => false, - } - } -} - impl Default for Elevator { fn default() -> Self { @@ -43,7 +31,11 @@ impl Elevator { //right.follow(&left, true); - Self { left, right, target: ElevatorPosition::Bottom } + Self { + left, + right, + target: ElevatorPosition::Bottom, + } } /// Set the stored goal position. /// TODO: update the below lines of this comment once controls scheme is figured out. @@ -55,18 +47,20 @@ impl Elevator { /// Get the stored goal position. /// For stuff like LEDs. /// Indexer used to reference this to get scoring speed, might need to go back to that once we have real bumpers - pub fn get_target(&self) -> ElevatorPosition {self.target} + pub fn get_target(&self) -> ElevatorPosition { + self.target + } /// Runs a trapezoidal (motion magic) profile on the elevator krakens to move the elevator to its stored target position. /// Most of the interesting stuff for this is in the elevator krakens' configurations (set in pheonix tuner). /// Those configuration files have been saved to Documents on the driver station laptop. pub fn run_to_target_trapezoid(&mut self) { //load position to run to in rotations from constants.rs - let target_position = match self.get_target(){ - ElevatorPosition::Bottom => {elevator::BOTTOM}, - ElevatorPosition::L2 => {elevator::L2}, - ElevatorPosition::L3 => {elevator::L3}, - ElevatorPosition::L4 => {elevator::L4}, + let target_position = match self.get_target() { + ElevatorPosition::Bottom => elevator::BOTTOM, + ElevatorPosition::L2 => elevator::L2, + ElevatorPosition::L3 => elevator::L3, + ElevatorPosition::L4 => elevator::L4, }; // current implementation is to just set the control mode and call this function every frame