Skip to content

10k #17

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 14 commits into from
Mar 29, 2025
Merged

10k #17

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
14 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
2b374b8
channges from weekend
Sha-dos Mar 27, 2025
2c43b10
climber and auto
Sha-dos Mar 27, 2025
5d65c5c
constants changes
Sha-dos Mar 28, 2025
5ce8785
untested right 2 auto
Sha-dos Mar 28, 2025
bcedd7a
added right auto to dashboard
Sha-dos Mar 28, 2025
eb3cb6b
tuned lineup
Sha-dos Mar 28, 2025
5395b94
added override to score
Sha-dos Mar 28, 2025
83df134
upped intake speed
Sha-dos Mar 29, 2025
4a56206
upped L3 speed
Sha-dos Mar 29, 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
259 changes: 259 additions & 0 deletions auto/Right2.traj
View file
Open in desktop

Large diffs are not rendered by default.

131 changes: 112 additions & 19 deletions src/auto/mod.rs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,7 @@ use uom::si::f64::Angle;

use crate::subsystems::{Drivetrain, Elevator, ElevatorPosition, Indexer, LineupSide};
use crate::{constants, score, Ferris};
use crate::constants::elevator::L3_ALGAE;
use crate::constants::indexer::{BOTTOM_SPEED, INTAKE_SPEED, L2_SPEED, L3_SPEED, L4_SPEED, LASER_TRIP_DISTANCE_MM};

#[derive(Serialize, Deserialize)]
Expand All @@ -28,6 +29,7 @@ pub enum Auto {
RotationTest,
BlueMidLeft2,
TushPush1,
Right2,
}

impl Auto {
Expand All @@ -41,6 +43,7 @@ impl Auto {
"RotationTest" => Auto::RotationTest,
"BlueMidLeft2" => Auto::BlueMidLeft2,
"TushPush1" => Auto::TushPush1,
"Right2" => Auto::Right2,
_ => Auto::Nothing,
}
}
Expand All @@ -55,6 +58,7 @@ impl Auto {
Auto::RotationTest => "RotationTest",
Auto::BlueMidLeft2 => "BlueMidLeft2",
Auto::TushPush1 => "TushPush1",
Auto::Right2 => "Right2",
_ => "none",
}
}
Expand All @@ -69,6 +73,7 @@ impl Auto {
// Auto::RotationTest,
Auto::BlueMidLeft2,
Auto::TushPush1,
Auto::Right2,
]
}

Expand All @@ -95,6 +100,7 @@ impl Auto {
Auto::RotationTest => rotation_test(Rc::clone(&ferris)).await.expect("Failed running auto"),
Auto::BlueMidLeft2 => blue_mid_left_2(Rc::clone(&ferris)).await.expect("Failed running auto"),
Auto::TushPush1 => tush_push_1(Rc::clone(&ferris)).await.expect("Failed running auto"),
Auto::Right2 => right_2(Rc::clone(&ferris)).await.expect("Failed running auto"),
}
}
}
Expand Down Expand Up @@ -141,10 +147,11 @@ pub async fn async_score(
ElevatorPosition::L2 => L2_SPEED,
ElevatorPosition::L3 => L3_SPEED,
ElevatorPosition::L4 => L4_SPEED,
ElevatorPosition::L3Algae => L3_SPEED
};
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 @@ -238,18 +245,11 @@ pub async fn blue_2(robot: Rc<RefCell<Ferris>>) -> Result<(), Box<dyn std::error
));

join!(drive("Blue2", &mut drivetrain, 1), async {
elevator.set_target(ElevatorPosition::L2);
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;
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 @@ -423,18 +423,76 @@ async fn tush_push_1(robot: Rc<RefCell<Ferris>>) -> Result<(), Box<dyn std::erro
drive("TushPush1", &mut drivetrain, 1).await?;

join!(drive("TushPush1", &mut drivetrain, 2), async {
elevator.set_target(ElevatorPosition::L2);
elevator.set_target(ElevatorPosition::L4);
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;
indexer.stop();
let _ = timeout(Duration::from_secs_f64(1.25), async {
loop {
drivetrain.update_limelight().await;
drivetrain.post_odo().await;

sleep(Duration::from_millis(20)).await;
}
}).await;

elevator.set_target(ElevatorPosition::L4);

join!(
timeout(Duration::from_secs_f64(2.), async {
loop {
drivetrain.update_limelight().await;
drivetrain.post_odo().await;

if drivetrain.lineup(LineupSide::Right,
ElevatorPosition::L4,
robot.dt,
if alliance_station().red() {Some(10)} else { Some(21)}).await
{
break;
}

sleep(Duration::from_millis(20)).await;
}
}),
elevator.run_to_target_trapezoid_async()
);

drivetrain.stop();

indexer.set_speed(-0.4);

wait(|| !indexer.is_laser_tripped()).await;

sleep(Duration::from_secs_f64(1.5)).await;
indexer.stop();

Ok(())
}

pub async fn right_2(robot: Rc<RefCell<Ferris>>) -> Result<(), Box<dyn std::error::Error>> {
let mut robot_ref = robot.borrow_mut();
let mut drivetrain = robot_ref.drivetrain.deref().borrow_mut();
let mut elevator = robot_ref.elevator.deref().borrow_mut();
let mut indexer = robot_ref.indexer.deref().borrow_mut();

drivetrain.reset_heading_offset(
if alliance_station().red() {
Angle::new::<degree>(180.)
} else {
Angle::new::<degree>(0.)
});
drivetrain.odometry.set(Vector2::new(
Length::new::<meter>(7.18402099609375),
Length::new::<meter>(2.696911096572876),
));

join!(drive("Right2", &mut drivetrain, 1), async {
elevator.set_target(ElevatorPosition::L4);
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;
drivetrain.post_odo().await;
Expand All @@ -449,8 +507,43 @@ async fn tush_push_1(robot: Rc<RefCell<Ferris>>) -> Result<(), Box<dyn std::erro
&mut elevator,
&mut indexer,
ElevatorPosition::L4,
robot.dt,
if alliance_station().red() {Some(10)} else { Some(21)}
robot_ref.dt,
if alliance_station().red() {Some(8)} else { Some(17)}
)
.await;

join!(drive("Right2", &mut drivetrain, 3), async {
elevator.set_target(ElevatorPosition::Bottom);
elevator.run_to_target_trapezoid_async().await;

indexer.set_speed(INTAKE_SPEED);

wait(|| indexer.is_laser_tripped()).await;

indexer.stop();
});

join!(drive("Right2", &mut drivetrain, 4), async {
sleep(Duration::from_secs_f64(1.25)).await;
elevator.set_target(ElevatorPosition::L4);
elevator.run_to_target_trapezoid();
});

let _ = timeout(Duration::from_secs_f64(0.5), async {
loop {
drivetrain.update_limelight().await;
sleep(Duration::from_millis(20)).await;
}
}).await;

async_score(
&mut drivetrain,
LineupSide::Left,
&mut elevator,
&mut indexer,
ElevatorPosition::L4,
robot_ref.dt,
if alliance_station().red() {Some(8)} else { Some(17)}
)
.await;

Expand Down
43 changes: 26 additions & 17 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,29 +55,29 @@ 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 = -35.2; //Last measured 3/27 @ 9:27: -36.8, -37.3, -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.;
pub const TX_FUDGE_FACTOR: f64 = 0.165 / 20.;
}

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.;

pub const PIGEON_OFFSET: f64 = -0.4;

pub const SWERVE_TURN_KP: f64 = 0.6;

pub const SWERVE_ROTATIONS_TO_INCHES: f64 = (1. / 6.75) * (3.65 * PI);
pub const SWERVE_ROTATIONS_TO_INCHES: f64 = (1. / 6.75) * (3.75 * PI);
pub const SWERVE_TURN_RATIO: f64 = 12.8;

pub const SWERVE_DRIVE_KP: f64 = 0.7;
Expand All @@ -100,10 +102,13 @@ pub mod drivetrain {
pub const TY_ACCEPTABLE_ERROR: f64 = 1.8;
pub const YAW_ACCEPTABLE_ERROR: f64 = 0.02;

pub const LINEUP_DRIVE_KP: f64 = 1.;
pub const LINEUP_DRIVE_KP: f64 = 1.1;
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,26 @@ 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 L3: f64 = 13.; // unit is rotations
pub const L4: f64 = 39.5; // unit is rotations
pub const L2: f64 = 0.05; // unit is rotations
pub const L3: f64 = 12.5; // unit is rotations
pub const L4: f64 = 42.9; // unit is rotations
pub const L3_ALGAE: f64 = 13.; // 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 INTAKE_SPEED: f64 = -0.25;
pub const LASER_TRIP_DISTANCE_MM: i32 = 10;
pub const INDEXER_LASER_DEBOUNCE_TIME_SECONDS: f64 = 0.08;
pub const INTAKE_SPEED: f64 = -0.45;
pub const BOTTOM_SPEED: f64 = -0.35;
pub const L2_SPEED: f64 = -0.4;
pub const L3_SPEED: f64 = -0.4;
pub const L3_SPEED: f64 = -0.6;
pub const L4_SPEED: f64 = -0.4;
}
pub mod climber {
pub const CLIMB_SPEED: f64 = 0.5;
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 +155,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
Loading
Loading