Skip to content

Wonky #16

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
Sha-dos merged 5 commits into from
Mar 29, 2025
Merged

Wonky #16

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
75354e2
wonky L4 setpoin
TheSnowHatHero Mar 21, 2025
2423439
canrange in drivetrain, post to telemetry
TheSnowHatHero Mar 21, 2025
4a7ab00
wonky climber
TheSnowHatHero Mar 21, 2025
7ac270b
debouncer & tuning
TheSnowHatHero Mar 22, 2025
399fb28
auto adjustment and such
TheSnowHatHero Mar 22, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
14 changes: 7 additions & 7 deletions src/auto/mod.rs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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();
Expand Down Expand Up @@ -242,14 +242,14 @@ pub async fn blue_2(robot: Rc<RefCell<Ferris>>) -> Result<(), Box<dyn std::error
elevator.run_to_target_trapezoid();

indexer.set_speed(INTAKE_SPEED);
wait(|| indexer.get_laser_dist() < LASER_TRIP_DISTANCE_MM && indexer.get_laser_dist() != -1).await;
wait(|| indexer.is_laser_tripped()).await;
indexer.stop();

elevator.set_target(ElevatorPosition::L4);
elevator.run_to_target_trapezoid();
});

let _ = timeout(Duration::from_secs_f64(1.5), async {
let _ = timeout(Duration::from_secs_f64(0.5), async {
loop {
drivetrain.update_limelight().await;
drivetrain.post_odo().await;
Expand All @@ -275,7 +275,7 @@ pub async fn blue_2(robot: Rc<RefCell<Ferris>>) -> Result<(), Box<dyn std::error

indexer.set_speed(INTAKE_SPEED);

wait(|| indexer.get_laser_dist() < LASER_TRIP_DISTANCE_MM && indexer.get_laser_dist() != -1).await;
wait(|| indexer.is_laser_tripped()).await;

indexer.stop();
});
Expand All @@ -286,7 +286,7 @@ pub async fn blue_2(robot: Rc<RefCell<Ferris>>) -> Result<(), Box<dyn std::error
elevator.run_to_target_trapezoid();
});

let _ = timeout(Duration::from_secs_f64(1.25), async {
let _ = timeout(Duration::from_secs_f64(0.5), async {
loop {
drivetrain.update_limelight().await;
sleep(Duration::from_millis(20)).await;
Expand Down Expand Up @@ -370,7 +370,7 @@ async fn blue_mid_left_2(robot: Rc<RefCell<Ferris>>) -> Result<(), Box<dyn std::

indexer.set_speed(INTAKE_SPEED);

wait(|| indexer.get_laser_dist() < LASER_TRIP_DISTANCE_MM && indexer.get_laser_dist() != -1).await;
wait(|| indexer.is_laser_tripped()).await;

indexer.stop();
});
Expand Down Expand Up @@ -427,7 +427,7 @@ async fn tush_push_1(robot: Rc<RefCell<Ferris>>) -> Result<(), Box<dyn std::erro
elevator.run_to_target_trapezoid();

indexer.set_speed(INTAKE_SPEED);
wait(|| indexer.get_laser_dist() < LASER_TRIP_DISTANCE_MM && indexer.get_laser_dist() != -1).await;
wait(|| indexer.is_laser_tripped()).await;
indexer.stop();

elevator.set_target(ElevatorPosition::L4);
Expand Down
30 changes: 19 additions & 11 deletions src/constants.rs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 {
Expand All @@ -32,12 +35,11 @@ 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 {
pub const RAISE: i32 = 1;
pub const GRAB: i32 = 0;
pub const CLIMBER_MOTOR_ID: i32 = 20;
}

pub mod led {
Expand All @@ -53,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<f64> = 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.;
}
Expand All @@ -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.;

Expand Down Expand Up @@ -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;
Expand All @@ -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;
Expand All @@ -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;
Expand Down
2 changes: 1 addition & 1 deletion src/lib.rs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down
45 changes: 10 additions & 35 deletions src/main.rs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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};

Expand Down Expand Up @@ -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 {
Expand All @@ -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.);
}
}
}
47 changes: 6 additions & 41 deletions src/subsystems/climber.rs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 {
Expand All @@ -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);
}
}
Loading
Loading