Skip to content

Commit

Permalink
pose testing
Browse files Browse the repository at this point in the history
  • Loading branch information
PGgit08 committed Mar 23, 2024
1 parent ec5ac1d commit 24e51b3
Showing 3 changed files with 20 additions and 4 deletions.
19 changes: 16 additions & 3 deletions src/main/java/frc/robot/Constants.java
Original file line number Diff line number Diff line change
@@ -149,8 +149,21 @@ public static class Presets {
public static final double ELEVATOR_AMP_HANDOFF = 0.04;

// TODO: get values for these
public static final InterpolatingDoubleTreeMap SHOOTER_DISTANCE_ANGLE = new InterpolatingDoubleTreeMap() {};
public static final InterpolatingDoubleTreeMap ELEVATOR_DISTANCE_HEIGHT = new InterpolatingDoubleTreeMap() {};
public static final InterpolatingDoubleTreeMap SHOOTER_DISTANCE_ANGLE = new InterpolatingDoubleTreeMap();

static {
SHOOTER_DISTANCE_ANGLE.put(1.755, 43.78);

SHOOTER_DISTANCE_ANGLE.put(3.500, 28.27);
};

public static final InterpolatingDoubleTreeMap ELEVATOR_DISTANCE_HEIGHT = new InterpolatingDoubleTreeMap();

static {
ELEVATOR_DISTANCE_HEIGHT.put(1.755, 0.061);

ELEVATOR_DISTANCE_HEIGHT.put(3.500, 0.010);
}
}

public static class Ports {
@@ -168,7 +181,7 @@ public static class FieldConstants {

public static final double SHOOTER_SLOW_THRESHOLD = 2;

public static final double TAG_DISTANCE_THRESHOLD = 3;
public static final double TAG_DISTANCE_THRESHOLD = 3.5;
public static final double TAG_DISTANCE_RESET_THRESHOLD = 1;

public static final int SPEAKER_TAG_BLUE = 7;
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/RobotContainer.java
Original file line number Diff line number Diff line change
@@ -135,7 +135,7 @@ private void configureBindings() {
_operatorController.L2().whileTrue(new SpinShooter(_shooterSubsystem, ShooterState.AMP).handleInterrupt(stopShooter));
_operatorController.R2().whileTrue(new SpinShooter(_shooterSubsystem, ShooterState.INTAKE).handleInterrupt(stopShooter));

_operatorController.square().whileTrue(safeFeedIn);
_operatorController.square().whileTrue(new FeedActuate(_intakeSubsystem, ActuatorState.OUT, FeedMode.INTAKE));
_operatorController.circle().whileTrue(feedOut);
_operatorController.triangle().whileTrue(new FeedActuate(_intakeSubsystem, ActuatorState.STOWED, FeedMode.OUTTAKE));
_operatorController.cross().whileTrue(new FeedActuate(_intakeSubsystem, FeedMode.INTAKE));
3 changes: 3 additions & 0 deletions src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java
Original file line number Diff line number Diff line change
@@ -166,6 +166,9 @@ public void periodic() {
SmartDashboard.putNumber("Match Time", DriverStation.getMatchTime());
SmartDashboard.putNumber("Shot Distance", shotVector().getNorm());

SmartDashboard.putNumber("ROBOT X POSE", getPose().getX());
SmartDashboard.putNumber("ROBOT Y POSE", getPose().getY());

// Update the bot's pose
_estimator.update(getHeadingRaw(), new SwerveModulePosition[]{
_frontLeft.getPosition(),

0 comments on commit 24e51b3

Please sign in to comment.