Skip to content
Open
Show file tree
Hide file tree
Changes from all commits
Commits
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
21 changes: 8 additions & 13 deletions src/main/java/frc/robot/Autos.java
Original file line number Diff line number Diff line change
Expand Up @@ -31,7 +31,6 @@
import frc.robot.generated.ChoreoVars;
import frc.robot.subsystems.drive.Drive;
import frc.robot.subsystems.intake.Intake;
import frc.robot.subsystems.shooter.Shooter;
import frc.robot.util.Utilities;

@Logged
Expand Down Expand Up @@ -152,8 +151,6 @@ private StartPosition(String displayName, Pose2d blueStartPose, AutoDriveOption.
@NotLogged
private final Drive _driveSubsystem;
@NotLogged
private final Shooter _shooterSubsystem;
@NotLogged
private final Intake _intakeSubsystem;
@NotLogged
private final AutoFactory _autoFactory;
Expand Down Expand Up @@ -182,10 +179,9 @@ private StartPosition(String displayName, Pose2d blueStartPose, AutoDriveOption.
@Logged
private Pose2d _currentDesiredPathPose = new Pose2d();

public Autos(Drive driveSubsystem, Shooter shooterSubsystem, Intake intakeSubsystem)
public Autos(Drive driveSubsystem, Intake intakeSubsystem)
{
_driveSubsystem = driveSubsystem;
_shooterSubsystem = shooterSubsystem;
_intakeSubsystem = intakeSubsystem;
_autoFactory = new AutoFactory(() -> driveSubsystem.getState().Pose, driveSubsystem::resetPose, this::followTrajectory, true, driveSubsystem);
_trajectoryDrawRoutine = _autoFactory.newRoutine("Trajectory Drawing");
Expand Down Expand Up @@ -223,7 +219,6 @@ public Command buildAuto()
var driveOption = _driveChooser.getSelected();
var startPose = getStartPose(mode, startPosition, driveOption);
var resetPose = Commands.runOnce(() -> _driveSubsystem.resetPose(startPose));
var shoot = _shooterSubsystem.shoot();
var delay = Commands.waitSeconds(delaySeconds);
var extend = _intakeSubsystem.getExtendCmd();
var pickup = _intakeSubsystem.runRollersForward();
Expand All @@ -236,13 +231,13 @@ public Command buildAuto()
{
case DoNothing -> resetPose;
case DriveOnly -> Commands.sequence(resetPose, drive);
case ShootOnly -> Commands.sequence(resetPose, shoot);
case ShootWithDelay -> Commands.sequence(resetPose, delay, shoot);
case DriveThenShoot -> Commands.sequence(resetPose, drive, shoot);
case ShootWithDelayThenDrive -> Commands.sequence(resetPose, delay, shoot, drive);
case HubOutpostDelayTowerShoot -> Commands.sequence(resetPose, hubToOutpost, Commands.waitSeconds(HUB_OUTPOST_TOWER_DELAY), outpostToTower, shoot);
case DrivePickupThenShoot -> Commands.sequence(resetPose, extend, Commands.deadline(drive, pickup), shoot.alongWith(jiggle));
case DriveWithDelayPickupThenShoot -> Commands.sequence(resetPose, delay, extend, Commands.deadline(drive, pickup), shoot.alongWith(jiggle));
case ShootOnly -> Commands.sequence(resetPose);
case ShootWithDelay -> Commands.sequence(resetPose, delay);
case DriveThenShoot -> Commands.sequence(resetPose, drive);
case ShootWithDelayThenDrive -> Commands.sequence(resetPose, delay, drive);
case HubOutpostDelayTowerShoot -> Commands.sequence(resetPose, hubToOutpost, Commands.waitSeconds(HUB_OUTPOST_TOWER_DELAY), outpostToTower);
case DrivePickupThenShoot -> Commands.sequence(resetPose, extend, Commands.deadline(drive, pickup), jiggle);
case DriveWithDelayPickupThenShoot -> Commands.sequence(resetPose, delay, extend, Commands.deadline(drive, pickup), jiggle);
};
}

Expand Down
154 changes: 6 additions & 148 deletions src/main/java/frc/robot/Constants.java
Original file line number Diff line number Diff line change
Expand Up @@ -8,18 +8,14 @@
import static edu.wpi.first.units.Units.RPM;
import static edu.wpi.first.units.Units.RadiansPerSecond;
import static edu.wpi.first.units.Units.Meters;
import static edu.wpi.first.units.Units.MetersPerSecond;
import static edu.wpi.first.units.Units.Milliseconds;
import static edu.wpi.first.units.Units.Percent;
import static edu.wpi.first.units.Units.Rotations;
import static edu.wpi.first.units.Units.RotationsPerSecond;
import static edu.wpi.first.units.Units.RotationsPerSecondPerSecond;
import static edu.wpi.first.units.Units.Seconds;
import static edu.wpi.first.units.Units.Volts;

import java.util.List;
import java.util.Map;
import java.util.stream.Stream;

import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.VecBuilder;
Expand All @@ -28,15 +24,11 @@
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap;

import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N3;
import edu.wpi.first.units.AngleUnit;
import edu.wpi.first.units.AngularAccelerationUnit;
import edu.wpi.first.units.AngularVelocityUnit;
import edu.wpi.first.units.DistanceUnit;
import edu.wpi.first.units.VoltageUnit;
import edu.wpi.first.units.measure.Angle;
import edu.wpi.first.units.measure.AngularVelocity;
import edu.wpi.first.units.measure.Current;
Expand Down Expand Up @@ -159,146 +151,6 @@ public static class GeneralConstants
public static final Translation2d FIELD_CENTER = new Translation2d(FIELD_SIZE_X.div(2), FIELD_SIZE_Y.div(2));
}

public static class ShooterConstants
{
// Flywheel
public static final double FLYWHEEL_KP = 0.00015; // TODO: Tune
public static final double FLYWHEEL_KI = 0.0; // TODO: Tune
public static final double FLYWHEEL_KD = 0.0005;
public static final Voltage FLYWHEEL_KS = Volts.of(0.0); // TODO: Tune - static friction voltage
public static final Per<VoltageUnit, AngularVelocityUnit> FLYWHEEL_KV = Volts.of(12.0).div(RPM.of(6784.0));
public static final Per<VoltageUnit, AngularAccelerationUnit> FLYWHEEL_KA = Volts.of(0).per(RotationsPerSecondPerSecond); // TODO: Tune - acceleration voltage
public static final Dimensionless FLYWHEEL_TOLERANCE = Percent.of(15);
public static final Current FLYWHEEL_CURRENT_LIMIT = Amps.of(60);
public static final AngularVelocity MANUAL_SHOOT_RPM = RPM.of(3500.0);
public static final AngularVelocity PASS_FLYWHEEL_VELOCITY = RPM.of(3000.0); // TODO: Tune

// Turret
public static final Current TURRET_CURRENT_LIMIT = Amps.of(60.0);
public static final double TURRET_KP = 0.19;
public static final double TURRET_KI = 0.01;
public static final double TURRET_KD = 0.0115;
public static final Voltage TURRET_STATIC_FF = Volts.of(0.22);
public static final Angle TURRET_STATIC_FF_ERROR_DEADBAND = Degrees.of(0.75);
public static final Voltage TURRET_MAX_OUTPUT_STEP_PER_LOOP = Volts.of(3.0);
public static final Angle TURRET_HARD_MIN_ANGLE = Degrees.of(-180.0);
public static final Angle TURRET_HARD_MAX_ANGLE = Degrees.of(180.0);
public static final boolean TURRET_SENSOR_INVERTED = true;
public static final Angle TURRET_POT_OFFSET = Degrees.of(-10.0);
public static final Angle TURRET_SOFT_MIN_ANGLE = Degrees.of(-160.0);
public static final Angle TURRET_SOFT_MAX_ANGLE = Degrees.of(160.0);
public static final Angle TURRET_HOME_ANGLE = Degrees.of(0.0);
public static final Angle TURRET_TOLERANCE = Degrees.of(1.25);
public static final Angle TURRET_LINED_UP_HOLD_TOLERANCE = Degrees.of(2.0);
public static final Angle TURRET_TARGET_SETPOINT_DEADBAND = Degrees.of(0.1);
public static final Angle TURRET_MAX_SETPOINT_STEP_PER_LOOP = Degrees.of(4.0);
public static final Angle TURRET_TRACK_COMMAND_DEADBAND = Degrees.of(0.5);
public static final Angle TURRET_TRACK_COMMAND_MAX_STEP_PER_LOOP = Degrees.of(4.0);
public static final Angle TURRET_TRACK_TX_DEADBAND = Degrees.of(1.0);
public static final boolean TURRET_USE_VISION_TX_TRIM = false;
public static final Time TURRET_VISION_TX_FILTER_WINDOW = Milliseconds.of(100.0);
public static final Angle TURRET_VISION_TX_MAX_STEP_PER_LOOP = Degrees.of(0.4);
public static final Angle TURRET_TRACK_BIAS = Degrees.zero(); // TODO: Tune static shot bias from log data
public static final boolean TURRET_USE_LINEAR_DRIFT_COMPENSATION = true;
public static final Distance TURRET_DRIFT_LATERAL_BIAS = Inches.of(11.060660171779821);
public static final Distance TURRET_DRIFT_LATERAL_SINE_AMPLITUDE = Inches.of(7.1890180494515015);
public static final Angle TURRET_DRIFT_LATERAL_PHASE_OFFSET = Degrees.of(-20.866427056070258);
public static final Angle TURRET_LINEAR_DRIFT_MAX_CORRECTION = Degrees.of(15.0);
public static final double TURRET_LINEAR_DRIFT_CORRECTION_SIGN = -1.0;
public static final String LIMELIGHT_NAME = "limelight-shooter";
public static final Angle TURRET_PASS_TARGET = Degrees.of(180.0); // TODO: Validate in driver practice
public static final Distance TURRET_CENTER_TAG_TO_HUB_CENTER = Inches.of(23.5);
public static final List<Integer> BLUE_HUB_TAG_IDS = List.of(18, 20, 21, 26);
public static final List<Integer> RED_HUB_TAG_IDS = List.of(2, 4, 5, 10);
public static final List<Integer> ALL_HUB_TAG_IDS = Stream.concat(BLUE_HUB_TAG_IDS.stream(), RED_HUB_TAG_IDS.stream()).toList();
public static final Translation3d CENTER_TAG_TO_HUB_CENTER_OFFSET = new Translation3d(Inches.of(-23.5), Inches.zero(), Inches.zero());
public static final Angle TURRET_ZERO_OFFSET_FROM_ROBOT_FORWARD = Degrees.of(90); // Hub "zero" is 90 degrees
// left of robot forward
public static final Translation2d BLUE_HUB = new Translation2d(Inches.of(182.1), Inches.of(158.85));
public static final Translation2d RED_HUB = new Translation2d(Inches.of(469.1), Inches.of(158.85));
public static final Translation2d ROBOT_TO_TURRET_PIVOT = new Translation2d(Inches.of(-0.5), Inches.of(5.75));
public static final Translation2d TURRET_PIVOT_TO_RELEASE = new Translation2d(Inches.of(1.0), Inches.zero()); // Measured
// release
// point
// is
// ~1
// in
// forward
// of
// the
// pivot
// (turret
// -90),
// not
// lateral
public static final Translation2d ROBOT_TO_TURRET_RELEASE = ROBOT_TO_TURRET_PIVOT.plus(TURRET_PIVOT_TO_RELEASE);
public static final Translation2d ROBOT_TO_TURRET_CAMERA = new Translation2d(Inches.zero(), Inches.of(8.0));
public static final Distance TURRET_LIMELIGHT_HEIGHT = Inches.of(24.625); // 24 5/8"
public static final Distance HUB_TAG_HEIGHT = Inches.of(44.25);
public static final Distance TURRET_TO_HUB_HEIGHT_DELTA = HUB_TAG_HEIGHT.minus(TURRET_LIMELIGHT_HEIGHT);
public static final Angle TURRET_LIMELIGHT_PITCH = Degrees.zero();
public static final Time SWM_RELEASE_PHASE_DELAY = Seconds.of(0.03);
public static final int SWM_LOOKAHEAD_ITERATIONS = 8;
public static final LinearVelocity SWM_ENABLE_TRANSLATIONAL_SPEED = MetersPerSecond.of(0.15);
public static final AngularVelocity SWM_ENABLE_ANGULAR_SPEED = DegreesPerSecond.of(10.0);
public static final LinearVelocity SWM_FEED_MAX_TRANSLATIONAL_SPEED = MetersPerSecond.of(1.5);
public static final AngularVelocity SWM_FEED_MAX_ANGULAR_SPEED = DegreesPerSecond.of(45.0);
public static final Angle SWM_FEED_TURRET_TOLERANCE = Degrees.of(5.0);
public static final Time SWM_FEED_STABILITY_WINDOW = Milliseconds.of(60.0);
private static final double SWM_MIN_TOF_DISTANCE_IN = 64.0;
private static final double SWM_MAX_TOF_DISTANCE_IN = 159.0;

// @formatter:off
private static final InterpolatingDoubleTreeMap FLYWHEEL_SPEED_TABLE = InterpolatingDoubleTreeMap.ofEntries
(
Map.entry(50.0, 2500.0),
Map.entry(75.5, 2950.0),
Map.entry(101.0, 3400.0),
Map.entry(113.0, 3850.0),
Map.entry(130.0, 3900.0),
Map.entry(151.0, 4600.0),
Map.entry(183.0, 5100.0),
Map.entry(204.0, 5850.0)
);

// Keep TOF as its own lookup so the moving-shot solve matches the empirical
// map-based pattern used by teams like 5000, 6328, and Eeshwar's writeup.
private static final InterpolatingDoubleTreeMap SHOT_TOF_TABLE = InterpolatingDoubleTreeMap.ofEntries
(
Map.entry(64.0, 0.542),
Map.entry(85.0, 1.06),
Map.entry(87.0, 1.02),
Map.entry(134.0, 1.22),
Map.entry(159.0, 1.74)
);
// @formatter:on

// Feeder
public static final Current FEEDER_CURRENT_LIMIT = Amps.of(60);
public static final Voltage FEEDER_VOLTAGE = Volts.of(6);
public static final Current ROTOR_CURRENT_LIMIT = Amps.of(80);
public static final Voltage ROTOR_FAST_VOLTAGE = Volts.of(8.0);
public static final Voltage ROTOR_MID_VOLTAGE = Volts.of(8.0);
public static final Voltage ROTOR_RETRACTED_VOLTAGE = Volts.of(8.0);

// TODO: Tune these values with testing!
public static AngularVelocity getFlywheelSpeedForDistance(Distance distance)
{
return RPM.of(FLYWHEEL_SPEED_TABLE.get(distance.in(Inches)));
}

public static Time getShotTimeOfFlight(Distance distance)
{
return Seconds.of(SHOT_TOF_TABLE.get(distance.in(Inches)));
}

public static boolean isMovingShotDistanceValid(Distance distance)
{
var distanceInches = distance.in(Inches);
return distanceInches >= SWM_MIN_TOF_DISTANCE_IN && distanceInches <= SWM_MAX_TOF_DISTANCE_IN;
}
}

public static class VisionConstants
{
public static final String LEFT_CAMERA_NAME = "limelight-left";
Expand Down Expand Up @@ -330,6 +182,12 @@ public static class VisionConstants

// Reject vision updates when no AprilTag is detected within this range
public static final Distance MAX_TARGET_DISTANCE = Feet.of(10);

// AprilTag constants
public static final List<Integer> BLUE_HUB_TAG_IDS = List.of(18, 20, 21, 26);
public static final List<Integer> RED_HUB_TAG_IDS = List.of(2, 4, 5, 10);
public static final Translation2d BLUE_HUB = new Translation2d(Inches.of(182.1), Inches.of(158.85));
public static final Translation2d RED_HUB = new Translation2d(Inches.of(469.1), Inches.of(158.85));
}

public static class ClimberConstants
Expand Down
13 changes: 0 additions & 13 deletions src/main/java/frc/robot/NetParams.java

This file was deleted.

Loading