diff --git a/src/constants.rs b/src/constants.rs index ea6a72b..82185f2 100644 --- a/src/constants.rs +++ b/src/constants.rs @@ -65,8 +65,33 @@ 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 = 0.0; - pub const L3: f64 = 0.0; - pub const L4: f64 = 39.7; + 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 +} +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; } diff --git a/src/lib.rs b/src/lib.rs index 80ffd54..c15e605 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -4,17 +4,17 @@ pub mod container; pub mod subsystems; pub mod swerve; -use std::cell::RefCell; - use crate::auto::Auto; use crate::container::control_drivetrain; use crate::subsystems::{ - Climber, Drivetrain, DrivetrainControlState, Elevator, Indexer, LineupSide, + 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; @@ -58,9 +58,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())), @@ -156,30 +156,87 @@ 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; } } - if let Ok(elevator) = self.elevator.try_borrow_mut() { - if self.controllers.operator.get(3) { - elevator.set_speed(0.5); - } else if self.controllers.operator.get(4) { - elevator.set_speed(-0.5); + 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) + { + elevator.set_target(ElevatorPosition::L2); + } 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) + { + elevator.set_target(ElevatorPosition::L4); + } + + // Setting the actual elevator operation + 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 + elevator.set_speed(-0.1); + } 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); } } if let Ok(indexer) = self.indexer.try_borrow_mut() { - if self.controllers.operator.get(2) { + if self + .controllers + .left_drive + .get(constants::joystick_map::INDEXER_OUT) + { + // Out the front indexer.set_speed(0.3); - } else if self.controllers.operator.get(1) { - indexer.set_speed(-0.1); + } 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); } @@ -214,7 +271,11 @@ 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); diff --git a/src/subsystems/drivetrain.rs b/src/subsystems/drivetrain.rs index 42c30ac..a5e17a0 100644 --- a/src/subsystems/drivetrain.rs +++ b/src/subsystems/drivetrain.rs @@ -224,12 +224,14 @@ 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; + 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 +303,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) } } diff --git a/src/subsystems/elevator.rs b/src/subsystems/elevator.rs index 9010e22..6e7ec0b 100644 --- a/src/subsystems/elevator.rs +++ b/src/subsystems/elevator.rs @@ -1,11 +1,16 @@ 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, } +#[derive(Copy, Clone, Debug, PartialEq)] pub enum ElevatorPosition { Bottom, L2, @@ -22,24 +27,58 @@ 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, + } + } + /// 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 } - 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), + /// 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); + self.left.set(ControlMode::MotionMagic, target_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) {