Skip to content

Commit

Permalink
minor change in hold commands
Browse files Browse the repository at this point in the history
  • Loading branch information
PGgit08 committed Jan 26, 2024
1 parent 1454f6e commit f58b573
Show file tree
Hide file tree
Showing 5 changed files with 14 additions and 6 deletions.
8 changes: 5 additions & 3 deletions src/main/java/frc/robot/commands/elevator/HoldElevator.java
Original file line number Diff line number Diff line change
Expand Up @@ -7,13 +7,15 @@
import frc.robot.subsystems.ElevatorSubsystem;

public class HoldElevator extends Command {
/** Creates a new HoldElevator. */
private ElevatorSubsystem _elevator;
private final ElevatorSubsystem _elevator;
private final double _maintainHeight;

/** Creates a new HoldElevator. */
public HoldElevator(ElevatorSubsystem elevator) {
// Use addRequirements() here to declare subsystem dependencies.
addRequirements(elevator);
_elevator = elevator;
_maintainHeight = elevator.getElevatorHeight();
}

// Called when the command is initially scheduled.
Expand All @@ -23,7 +25,7 @@ public void initialize() {}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
_elevator.setElevatorHeight(_elevator.getElevatorHeight()); // holds the elevator in place
_elevator.setElevatorHeight(_maintainHeight); // holds the elevator in place
}

// Called once the command ends or is interrupted.
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/commands/shooter/AutoAim.java
Original file line number Diff line number Diff line change
Expand Up @@ -20,7 +20,7 @@ public AutoAim(ShooterSubsystem shooter, VisionSubsystem vision, SwerveDriveSubs
_shooter = shooter;
_vision = vision;
_swerve = swerve;
addRequirements(_shooter, _vision, _swerve);
addRequirements(_shooter, _vision); // not adding swerve as a requirement to prevent interrupting default command
}

// Called when the command is initially scheduled.
Expand Down
4 changes: 3 additions & 1 deletion src/main/java/frc/robot/commands/shooter/HoldShooter.java
Original file line number Diff line number Diff line change
Expand Up @@ -8,12 +8,14 @@

public class HoldShooter extends Command {
private final ShooterSubsystem _shooter;
private final double _maintainAngle;

/** Creates a new HoldShooter. */
public HoldShooter(ShooterSubsystem shooter) {
// Use addRequirements() here to declare subsystem dependencies.

_shooter = shooter;
_maintainAngle = shooter.getAngle();

addRequirements(shooter);
}
Expand All @@ -25,7 +27,7 @@ public void initialize() {}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
_shooter.setAngle(_shooter.getAngle());
_shooter.setAngle(_maintainAngle);
}

// Called once the command ends or is interrupted.
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -394,6 +394,6 @@ public double shooterAngleToSpeaker() {
double zDifference = FieldConstants.SPEAKER_HEIGHT - Constants.Physical.SHOOTER_HEIGHT_STOWED;

double angle = Math.atan(zDifference / distanceToRobot);
return angle;
return Math.toDegrees(angle);
}
}
4 changes: 4 additions & 0 deletions src/main/java/frc/robot/utils/AllianceFieldConstants.java
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,10 @@

/** Dynamic field constants that at the start of the match are set based on the alliance color. */
public class AllianceFieldConstants {
/**
* The field layout.
* @see AprilTagFieldLayout
*/
public final AprilTagFieldLayout APRILTAG_LAYOUT = AprilTagFields.k2024Crescendo.loadAprilTagLayoutField();

/** The speaker's tag ID. */
Expand Down

0 comments on commit f58b573

Please sign in to comment.