diff --git a/src/auto/mod.rs b/src/auto/mod.rs index 54f5757..64e1104 100644 --- a/src/auto/mod.rs +++ b/src/auto/mod.rs @@ -144,7 +144,7 @@ pub async fn async_score( }; indexer.set_speed(indexer_speed); - wait(|| indexer.get_laser_dist() > LASER_TRIP_DISTANCE_MM || indexer.get_laser_dist() == -1).await; + wait(|| !indexer.is_laser_tripped()).await; sleep(Duration::from_secs_f64(0.2)).await; indexer.stop(); @@ -242,14 +242,14 @@ pub async fn blue_2(robot: Rc>) -> Result<(), Box>) -> Result<(), Box>) -> Result<(), Box>) -> Result<(), Box>) -> Result<(), Box = Vector2::new(11.118, 10.352); - // Increase distance by 10% for every 20 degrees of absolute value of tx + // Increase distance by 13.5% for every 20 degrees of absolute value of tx // Set this to 0 for new robots pub const TX_FUDGE_FACTOR: f64 = 0.135 / 20.; } @@ -67,7 +69,7 @@ pub mod drivetrain { use std::f64::consts::PI; pub const FR_OFFSET_DEGREES: f64 = 0.081299 * 360.; - pub const FL_OFFSET_DEGREES: f64 = 0.079102 * 360.; + pub const FL_OFFSET_DEGREES: f64 = 0.0942 * 360.; pub const BR_OFFSET_DEGREES: f64 = -0.056641 * 360.; pub const BL_OFFSET_DEGREES: f64 = 0.170898 * 360.; @@ -104,6 +106,9 @@ pub mod drivetrain { pub const LINEUP_DRIVE_KI: f64 = 10.; pub const LINEUP_DRIVE_KD: f64 = 10.; pub const LINEUP_DRIVE_IE: f64 = 0.25; + + pub const CANRANGE_DEBOUNCE_TIME_SECONDS: f64 = 0.02; + pub const REEF_SENSOR_TARGET_DISTANCE_METERS: f64 = 0.381; } pub mod pose_estimation { pub const ARC_ODOMETRY_MINIMUM_DELTA_THETA_RADIANS: f64 = 0.000001; @@ -117,20 +122,25 @@ pub mod pose_estimation { } pub mod elevator { pub const BOTTOM: f64 = 0.25; // unit is rotations - pub const L2: f64 = 1.5; // unit is rotations + pub const L2: f64 = 0.05; // unit is rotations pub const L3: f64 = 13.; // unit is rotations - pub const L4: f64 = 39.5; // unit is rotations + pub const L4: f64 = 42.9; // unit is rotations pub const ELEVATOR_TRAPEZOID_DT_MS: u64 = 50; // sleep.await this long in between updating the elevator trapezoidal when running its async function pub const POSITION_TOLERANCE: f64 = 0.25; // unit is rotations. finish elevator async move when within this distance of target } pub mod indexer { pub const LASER_TRIP_DISTANCE_MM: i32 = 2; + pub const INDEXER_LASER_DEBOUNCE_TIME_SECONDS: f64 = 0.08; pub const INTAKE_SPEED: f64 = -0.25; pub const BOTTOM_SPEED: f64 = -0.35; - pub const L2_SPEED: f64 = -0.4; + pub const L2_SPEED: f64 = -0.2; pub const L3_SPEED: f64 = -0.4; pub const L4_SPEED: f64 = -0.4; } +pub mod climber { + pub const CLIMB_SPEED: f64 = 0.3; + pub const FALL_SPEED: f64 = -0.3; +} pub mod joystick_map { // Joystick IDs (set in driver station) pub const RIGHT_DRIVE: i32 = 0; @@ -144,8 +154,6 @@ pub mod joystick_map { pub const RESET_HEADING: usize = 5; pub const CLIMB: usize = 2; pub const CLIMB_FALL: usize = 6; - pub const CLIMBER_GRAB: usize = 8; - pub const CLIMBER_RAISE: usize = 9; //Left drive pub const SLOW_MODE: usize = 1; diff --git a/src/lib.rs b/src/lib.rs index 8ca7104..19bbdab 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -343,7 +343,7 @@ pub fn score( let elevator_at_target = elevator.run_to_target_trapezoid(); if elevator_at_target && drivetrain_aligned { - if indexer.get_laser_dist() < constants::indexer::LASER_TRIP_DISTANCE_MM { + if indexer.is_laser_tripped() { let indexer_speed = match elevator_position { ElevatorPosition::Bottom => BOTTOM_SPEED, ElevatorPosition::L2 => L2_SPEED, diff --git a/src/main.rs b/src/main.rs index b774c5d..4e59cbf 100644 --- a/src/main.rs +++ b/src/main.rs @@ -15,10 +15,11 @@ use frcrs::telemetry::Telemetry; use tokio::task; use tokio::task::{AbortHandle, spawn_local}; use tokio::time::sleep; -use RobotCode2025::constants::joystick_map::{CLIMB, CLIMB_FALL, CLIMBER_GRAB, CLIMBER_RAISE, INTAKE, LINEUP_LEFT, LINEUP_RIGHT, SCORE_L2, SCORE_L3, SCORE_L4, WHEELS_ZERO}; +use RobotCode2025::constants::joystick_map::{CLIMB, CLIMB_FALL, INTAKE, LINEUP_LEFT, LINEUP_RIGHT, SCORE_L2, SCORE_L3, SCORE_L4, WHEELS_ZERO}; use RobotCode2025::container::control_drivetrain; use RobotCode2025::{constants, Ferris, score, TeleopState}; use RobotCode2025::auto::Auto; +use RobotCode2025::constants::climber::{CLIMB_SPEED, FALL_SPEED}; use RobotCode2025::constants::indexer::INTAKE_SPEED; use RobotCode2025::subsystems::{Climber, ElevatorPosition, LineupSide}; @@ -208,8 +209,7 @@ async fn teleop(robot: &mut Ferris) { elevator.set_target(ElevatorPosition::Bottom); elevator.run_to_target_trapezoid(); - if indexer.get_laser_dist() > constants::indexer::LASER_TRIP_DISTANCE_MM - || indexer.get_laser_dist() == -1 + if !indexer.is_laser_tripped() { indexer.set_speed(INTAKE_SPEED); } else { @@ -227,38 +227,13 @@ async fn teleop(robot: &mut Ferris) { } } - if robot.controllers.right_drive.get(CLIMB) { - if robot.climb_handle.is_none() { - let f = robot.clone(); - let climb_task = Climber::climb(f); - let handle = spawn_local(climb_task).abort_handle(); - robot.climb_handle = Some(handle); - } - } else if robot.controllers.right_drive.get(CLIMB_FALL) { - if let Some(handle) = robot.climb_handle.take() { - handle.abort(); - } - - if let Ok(mut climber) = robot.climber.try_borrow_mut() { - climber.fall() - } - } else { - if let Some(handle) = robot.climb_handle.take() { - handle.abort(); - } - - if let Ok(mut climber) = robot.climber.try_borrow_mut() { - if robot.controllers.right_drive.get(CLIMBER_RAISE) { - climber.set_raise(true); - } else { - climber.set_raise(false); - } - - if robot.controllers.right_drive.get(CLIMBER_GRAB) { - climber.set_grab(true); - } else { - climber.set_grab(false); - } + if let Ok(climber) = robot.climber.try_borrow_mut() { + if robot.controllers.right_drive.get(CLIMB) { + climber.set(CLIMB_SPEED); + } else if robot.controllers.right_drive.get(CLIMB_FALL) { + climber.set(FALL_SPEED); + } else { + climber.set(0.); } } } \ No newline at end of file diff --git a/src/subsystems/climber.rs b/src/subsystems/climber.rs index 989113f..f4e0b5b 100644 --- a/src/subsystems/climber.rs +++ b/src/subsystems/climber.rs @@ -2,11 +2,12 @@ use crate::constants::robotmap::climber::*; use crate::Ferris; use frcrs::solenoid::{ModuleType, Solenoid}; use std::time::Duration; +use frcrs::ctre::{ControlMode, Talon}; use tokio::time::sleep; +use crate::constants::robotmap; pub struct Climber { - raise: Solenoid, - grab: Solenoid, + motor: Talon } impl Default for Climber { @@ -18,47 +19,11 @@ impl Default for Climber { impl Climber { pub fn new() -> Self { Self { - raise: Solenoid::new(ModuleType::CTRE, RAISE), - grab: Solenoid::new(ModuleType::CTRE, GRAB), + motor: Talon::new(CLIMBER_MOTOR_ID, Some("can0".to_string())) } } - pub fn toggle_raise(&self) { - self.raise.toggle(); - } - - pub fn set_raise(&self, engaged: bool) { - self.raise.set(engaged); - } - - pub fn toggle_grab(&self) { - self.grab.toggle(); - } - - pub fn set_grab(&self, engaged: bool) { - self.grab.set(!engaged); - } - - pub async fn climb(ferris: Ferris) { - if let Ok(climber) = ferris.climber.try_borrow_mut() { - climber.set_raise(true); - sleep(Duration::from_secs_f64(1.75)).await; - climber.set_grab(true); - sleep(Duration::from_secs_f64(0.25)).await; - climber.set_raise(false); - } - } - - pub fn fall(&self) { - self.set_grab(false); - self.set_raise(false); - } - - pub async fn reverse_climb(&self) { - self.set_raise(true); - sleep(Duration::from_secs_f64(0.25)).await; - self.set_grab(false); - sleep(Duration::from_secs_f64(0.25)).await; - self.set_raise(false); + pub fn set(&self, speed: f64){ + self.motor.set(ControlMode::Percent, speed); } } diff --git a/src/subsystems/drivetrain.rs b/src/subsystems/drivetrain.rs index 0ce6297..8e42837 100644 --- a/src/subsystems/drivetrain.rs +++ b/src/subsystems/drivetrain.rs @@ -1,3 +1,4 @@ +use std::cmp::PartialEq; use std::collections::HashMap; use frcrs::alliance_station; use std::f64::consts::PI; @@ -6,9 +7,9 @@ use std::net::{IpAddr, Ipv4Addr, SocketAddr}; use std::ops::{Add, Sub}; use std::time::Duration; -use frcrs::ctre::{talon_encoder_tick, CanCoder, ControlMode, Pigeon, Talon}; +use frcrs::ctre::{talon_encoder_tick, CanCoder, ControlMode, Pigeon, Talon, CanRange}; -use crate::constants::drivetrain::{BL_OFFSET_DEGREES, BR_OFFSET_DEGREES, FL_OFFSET_DEGREES, FR_OFFSET_DEGREES, LINEUP_2D_TX_FWD_KP, LINEUP_2D_TX_STR_KP, LINEUP_2D_TY_FWD_KP, LINEUP_DRIVE_IE, LINEUP_DRIVE_KD, LINEUP_DRIVE_KI, LINEUP_DRIVE_KP, PIGEON_OFFSET, SWERVE_DRIVE_IE, SWERVE_DRIVE_KD, SWERVE_DRIVE_KI, SWERVE_DRIVE_KP, SWERVE_ROTATIONS_TO_INCHES, SWERVE_TURN_KP, SWERVE_TURN_RATIO, TARGET_TX_LEFT, TARGET_TX_RIGHT, TARGET_TY_LEFT, TARGET_TY_RIGHT, TX_ACCEPTABLE_ERROR, TY_ACCEPTABLE_ERROR, YAW_ACCEPTABLE_ERROR}; +use crate::constants::drivetrain::{BL_OFFSET_DEGREES, BR_OFFSET_DEGREES, CANRANGE_DEBOUNCE_TIME_SECONDS, FL_OFFSET_DEGREES, FR_OFFSET_DEGREES, LINEUP_2D_TX_FWD_KP, LINEUP_2D_TX_STR_KP, LINEUP_2D_TY_FWD_KP, LINEUP_DRIVE_IE, LINEUP_DRIVE_KD, LINEUP_DRIVE_KI, LINEUP_DRIVE_KP, PIGEON_OFFSET, REEF_SENSOR_TARGET_DISTANCE_METERS, SWERVE_DRIVE_IE, SWERVE_DRIVE_KD, SWERVE_DRIVE_KI, SWERVE_DRIVE_KP, SWERVE_ROTATIONS_TO_INCHES, SWERVE_TURN_KP, SWERVE_TURN_RATIO, TARGET_TX_LEFT, TARGET_TX_RIGHT, TARGET_TY_LEFT, TARGET_TY_RIGHT, TX_ACCEPTABLE_ERROR, TY_ACCEPTABLE_ERROR, YAW_ACCEPTABLE_ERROR}; use crate::constants::robotmap::swerve::*; use crate::swerve::kinematics::{ModuleState, Swerve}; use crate::swerve::odometry::{ModuleReturn, Odometry}; @@ -54,6 +55,58 @@ pub struct LineupLocation { forward_distance: Length, } +#[derive(Copy, Clone)] +pub enum DebounceType { + RISING, + FALLING, + BOTH +} + +/// https://docs.wpilib.org/en/stable/docs/software/advanced-controls/filters/debouncer.html +/// but in rust +pub struct Debouncer { + bounce_time: Duration, + debounce_type: DebounceType, + baseline: bool, + prev_time: Instant, +} + +impl Debouncer { + pub fn new(bounce_time: Duration, debounce_type: DebounceType) -> Debouncer { + Debouncer { + bounce_time, + debounce_type, + baseline: match debounce_type { + DebounceType::RISING => false, + DebounceType::FALLING => true, + DebounceType::BOTH => false, + }, + prev_time: Instant::now(), + } + } + fn reset_timer(&mut self) { + self.prev_time = Instant::now(); + } + fn has_elapsed(&self) -> bool { + self.prev_time.elapsed() >= self.bounce_time + } + /// copied line for line from wpilib + pub fn calculate(&mut self, input: bool) -> bool{ + if input == self.baseline { + self.reset_timer() + } + if self.has_elapsed() { + if matches!(self.debounce_type, DebounceType::BOTH) { + self.baseline = input; + self.reset_timer(); + } + input + } else { + self.baseline + } + } +} + pub struct Drivetrain { pigeon: Pigeon, @@ -73,6 +126,11 @@ pub struct Drivetrain { br_turn: Talon, br_encoder: CanCoder, + right_laser: CanRange, //from robot perspective + left_laser: CanRange, + right_laser_debouncer: Debouncer, + left_laser_debouncer: Debouncer, + kinematics: Swerve, pub odometry: Odometry, @@ -150,10 +208,10 @@ impl Drivetrain { /// default is: // side_distance: Length::new::(13. / 2.), // forward_distance: Length::new::(16.275), - lineup_locations.insert(17, LineupLocation { - side_distance: Length::new::(13. / 2.), - forward_distance: Length::new::(16.), - }); + // lineup_locations.insert(17, LineupLocation { + // side_distance: Length::new::(13. / 2.), + // forward_distance: Length::new::(16.), + // }); Self { pigeon: Pigeon::new(PIGEON, Some("can0".to_owned())), @@ -174,6 +232,11 @@ impl Drivetrain { br_turn, br_encoder, + right_laser: CanRange::new(RIGHT_LINEUP_LASER, Some("can0".to_owned())), + left_laser: CanRange::new(LEFT_LINEUP_LASER, Some("can0".to_owned())), + right_laser_debouncer: Debouncer::new(Duration::from_secs_f64(CANRANGE_DEBOUNCE_TIME_SECONDS), DebounceType::RISING), + left_laser_debouncer: Debouncer::new(Duration::from_secs_f64(CANRANGE_DEBOUNCE_TIME_SECONDS), DebounceType::RISING), + kinematics: Swerve::rectangle(Length::new::(21.5), Length::new::(21.5)), odometry: Odometry::new(), @@ -259,7 +322,7 @@ impl Drivetrain { ] } - pub async fn post_odo(&self) { + pub async fn post_odo(&mut self) { Telemetry::put_number( "odo_x", self.odometry @@ -287,6 +350,13 @@ impl Drivetrain { .get::(), ) .await; + Telemetry::put_number("left laser", self.left_laser.get_distance().get::()).await; + Telemetry::put_number("right laser", self.right_laser.get_distance().get::()).await; + let left_laser_tripped: String = if self.left_laser_debouncer.calculate(self.left_laser.get_distance().get::() < REEF_SENSOR_TARGET_DISTANCE_METERS && self.left_laser.get_distance().get::() > 0.) { "true".parse().unwrap() } else { "false".parse().unwrap() }; + Telemetry::put_string("left laser tripped", left_laser_tripped).await; + let left_laser_tripped: String = if self.right_laser_debouncer.calculate(self.right_laser.get_distance().get::() < REEF_SENSOR_TARGET_DISTANCE_METERS && self.right_laser.get_distance().get::() > 0.) { "true".parse().unwrap() } else { "false".parse().unwrap() }; + Telemetry::put_string("right laser tripped", left_laser_tripped).await; + } pub fn update_odo_absolute(&mut self, pose: Vector2) { diff --git a/src/subsystems/indexer.rs b/src/subsystems/indexer.rs index dba0857..0d8632b 100644 --- a/src/subsystems/indexer.rs +++ b/src/subsystems/indexer.rs @@ -7,11 +7,13 @@ use std::rc::Rc; use std::time::Duration; use std::time::Instant; use tokio::time::sleep; -use crate::constants::indexer::INTAKE_SPEED; +use crate::constants::indexer::{INDEXER_LASER_DEBOUNCE_TIME_SECONDS, INTAKE_SPEED, LASER_TRIP_DISTANCE_MM}; +use crate::subsystems::{DebounceType, Debouncer}; pub struct Indexer { motor: Talon, laser_can: LaserCan, + debouncer: Debouncer, } impl Default for Indexer { @@ -24,8 +26,9 @@ impl Indexer { pub fn new() -> Self { let motor = Talon::new(robotmap::indexer::MOTOR, None); let laser_can = LaserCan::new(robotmap::indexer::LASER_CAN); + let debouncer = Debouncer::new(Duration::from_secs_f64(INDEXER_LASER_DEBOUNCE_TIME_SECONDS), DebounceType::RISING); - Self { motor, laser_can } + Self { motor, laser_can, debouncer } } pub fn set_speed(&self, speed: f64) { @@ -33,9 +36,8 @@ impl Indexer { } pub async fn intake_coral(indexer: Rc>) { - if let Ok(indexer) = indexer.try_borrow_mut() { - while indexer.get_laser_dist() > constants::indexer::LASER_TRIP_DISTANCE_MM - || indexer.laser_can.get_measurement() == -1 + if let Ok(mut indexer) = indexer.try_borrow_mut() { + while !indexer.is_laser_tripped() { println!("Dist: {}", indexer.get_laser_dist()); indexer.set_speed(INTAKE_SPEED); @@ -49,6 +51,10 @@ impl Indexer { self.laser_can.get_measurement() } + pub fn is_laser_tripped(&mut self) -> bool { + self.debouncer.calculate(self.get_laser_dist() < LASER_TRIP_DISTANCE_MM && self.get_laser_dist() != -1) + } + pub fn stop(&self) { self.motor.stop(); }