From 75354e2df79669aa420f29938be27dd68fecd44c Mon Sep 17 00:00:00 2001 From: TheSnowHatHero Date: Thu, 20 Mar 2025 19:59:26 -0500 Subject: [PATCH 1/5] wonky L4 setpoin --- src/constants.rs | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/constants.rs b/src/constants.rs index ce4dd16..97f0ec4 100644 --- a/src/constants.rs +++ b/src/constants.rs @@ -22,6 +22,9 @@ pub mod robotmap { pub const BR_DRIVE: i32 = 7; pub const BR_TURN: i32 = 8; pub const BR_ENCODER: i32 = 16; + + pub const RIGHT_LINEUP_LASER:i32 = 18; + pub const LEFT_LINEUP_LASER:i32 = 19; } pub mod elevator { @@ -32,7 +35,7 @@ pub mod robotmap { pub mod indexer { pub const MOTOR: i32 = 12; pub const LASER_CAN: i32 = 0; // Cant save can id - + pub const INDEXER_775_MOTOR: i32 = 17; } pub mod climber { @@ -119,7 +122,7 @@ pub mod elevator { pub const BOTTOM: f64 = 0.25; // unit is rotations pub const L2: f64 = 1.5; // unit is rotations pub const L3: f64 = 13.; // unit is rotations - pub const L4: f64 = 39.5; // unit is rotations + pub const L4: f64 = 45.15; // 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 } From 24234394d5fd28e1f640a17b096a25ed0350fd7b Mon Sep 17 00:00:00 2001 From: TheSnowHatHero Date: Thu, 20 Mar 2025 20:00:13 -0500 Subject: [PATCH 2/5] canrange in drivetrain, post to telemetry --- src/subsystems/drivetrain.rs | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/src/subsystems/drivetrain.rs b/src/subsystems/drivetrain.rs index 0ce6297..a66a619 100644 --- a/src/subsystems/drivetrain.rs +++ b/src/subsystems/drivetrain.rs @@ -6,7 +6,7 @@ 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::robotmap::swerve::*; @@ -73,6 +73,9 @@ pub struct Drivetrain { br_turn: Talon, br_encoder: CanCoder, + right_laser: CanRange, //from robot perspective + left_laser: CanRange, + kinematics: Swerve, pub odometry: Odometry, @@ -174,6 +177,9 @@ 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())), + kinematics: Swerve::rectangle(Length::new::(21.5), Length::new::(21.5)), odometry: Odometry::new(), @@ -287,6 +293,9 @@ 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; + } pub fn update_odo_absolute(&mut self, pose: Vector2) { From 4a7ab00300707bf18335a22f32a4e814eeb84411 Mon Sep 17 00:00:00 2001 From: TheSnowHatHero Date: Thu, 20 Mar 2025 20:41:04 -0500 Subject: [PATCH 3/5] wonky climber --- src/constants.rs | 11 ++++----- src/main.rs | 42 ++++++++-------------------------- src/subsystems/climber.rs | 47 +++++---------------------------------- 3 files changed, 21 insertions(+), 79 deletions(-) diff --git a/src/constants.rs b/src/constants.rs index 97f0ec4..e51b278 100644 --- a/src/constants.rs +++ b/src/constants.rs @@ -39,8 +39,7 @@ pub mod robotmap { } pub mod climber { - pub const RAISE: i32 = 1; - pub const GRAB: i32 = 0; + pub const CLIMBER_MOTOR_ID: i32 = 20; } pub mod led { @@ -70,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.0983 * 360.; pub const BR_OFFSET_DEGREES: f64 = -0.056641 * 360.; pub const BL_OFFSET_DEGREES: f64 = 0.170898 * 360.; @@ -134,6 +133,10 @@ pub mod indexer { 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; @@ -147,8 +150,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/main.rs b/src/main.rs index b774c5d..fec8127 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}; @@ -227,38 +228,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); } } From 7ac270b5d762507a9421140a3dc2e8f549eb77b2 Mon Sep 17 00:00:00 2001 From: TheSnowHatHero Date: Sat, 22 Mar 2025 11:33:05 -0500 Subject: [PATCH 4/5] debouncer & tuning --- src/constants.rs | 10 ++++-- src/subsystems/drivetrain.rs | 65 ++++++++++++++++++++++++++++++++++-- 2 files changed, 70 insertions(+), 5 deletions(-) diff --git a/src/constants.rs b/src/constants.rs index e51b278..1725975 100644 --- a/src/constants.rs +++ b/src/constants.rs @@ -55,12 +55,12 @@ pub const HALF_FIELD_LENGTH_METERS: f64 = 8.05 / 2.; pub mod vision { use nalgebra::Vector2; - pub const LIMELIGHT_UPPER_PITCH_DEGREES: f64 = -34.5; //Last measured: -37.0 + pub const LIMELIGHT_UPPER_PITCH_DEGREES: f64 = -36.4; //Last measured: -37.3 past -37.0 pub const LIMELIGHT_UPPER_YAW_DEGREES: f64 = 90.; // Counterclockwise positive pub const LIMELIGHT_UPPER_HEIGHT_INCHES: f64 = 20.92; pub const ROBOT_CENTER_TO_LIMELIGHT_UPPER_INCHES: Vector2 = 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.; } @@ -69,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.0983 * 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.; @@ -106,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; @@ -127,6 +130,7 @@ pub mod elevator { } pub mod indexer { pub const LASER_TRIP_DISTANCE_MM: i32 = 2; + pub const LASERCAN_DEBOUNCE_TIME_SECONDS: f64 = 0.3; pub const INTAKE_SPEED: f64 = -0.25; pub const BOTTOM_SPEED: f64 = -0.35; pub const L2_SPEED: f64 = -0.4; diff --git a/src/subsystems/drivetrain.rs b/src/subsystems/drivetrain.rs index a66a619..e27665b 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; @@ -8,7 +9,7 @@ use std::time::Duration; 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, @@ -75,6 +128,8 @@ pub struct Drivetrain { right_laser: CanRange, //from robot perspective left_laser: CanRange, + right_laser_debouncer: Debouncer, + left_laser_debouncer: Debouncer, kinematics: Swerve, pub odometry: Odometry, @@ -179,6 +234,8 @@ impl Drivetrain { 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(), @@ -265,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 @@ -295,6 +352,10 @@ impl Drivetrain { .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("left_laser_tripped", left_laser_tripped).await; } From 399fb28c549a21944f394fbb26ec36dd3e1c2f38 Mon Sep 17 00:00:00 2001 From: TheSnowHatHero Date: Sat, 22 Mar 2025 18:25:47 -0500 Subject: [PATCH 5/5] auto adjustment and such --- src/auto/mod.rs | 14 +++++++------- src/constants.rs | 8 ++++---- src/lib.rs | 2 +- src/main.rs | 3 +-- src/subsystems/drivetrain.rs | 12 ++++++------ src/subsystems/indexer.rs | 16 +++++++++++----- 6 files changed, 30 insertions(+), 25 deletions(-) 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 BOTTOM_SPEED, ElevatorPosition::L2 => L2_SPEED, diff --git a/src/main.rs b/src/main.rs index fec8127..4e59cbf 100644 --- a/src/main.rs +++ b/src/main.rs @@ -209,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 { diff --git a/src/subsystems/drivetrain.rs b/src/subsystems/drivetrain.rs index e27665b..8e42837 100644 --- a/src/subsystems/drivetrain.rs +++ b/src/subsystems/drivetrain.rs @@ -208,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())), @@ -353,9 +353,9 @@ impl Drivetrain { 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; + 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("left_laser_tripped", left_laser_tripped).await; + Telemetry::put_string("right laser tripped", left_laser_tripped).await; } 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(); }