Skip to content

Commit

Permalink
[Robot] Bind shooter buttons to commands
Browse files Browse the repository at this point in the history
  • Loading branch information
@mvog2501
mvog2501 committed Jul 18, 2024
1 parent eb36624 commit fe0b139
Show file tree
Hide file tree
Showing 2 changed files with 124 additions and 110 deletions.
49 changes: 13 additions & 36 deletions Robot/src/main/java/com/swrobotics/robot/control/ControlBoard.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,15 +16,10 @@
import com.swrobotics.robot.subsystems.speaker.IntakeSubsystem;
import com.swrobotics.robot.subsystems.speaker.PivotSubsystem;
import com.swrobotics.robot.subsystems.speaker.ShooterSubsystem;
import com.swrobotics.robot.subsystems.speaker.aim.AmpAimCalculator;
import com.swrobotics.robot.subsystems.speaker.aim.LobCalculator;
import com.swrobotics.robot.subsystems.speaker.aim.LowLobAimCalculator;
import com.swrobotics.robot.subsystems.speaker.aim.ManualAimCalculator;
import com.swrobotics.robot.subsystems.speaker.aim.TableAimCalculator;
import com.swrobotics.robot.subsystems.swerve.SwerveDrive;
import com.swrobotics.robot.subsystems.swerve.SwerveDrive.TurnRequest;

import edu.wpi.first.math.filter.Debouncer;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
Expand Down Expand Up @@ -72,11 +67,6 @@ public class ControlBoard extends SubsystemBase {

private final Trigger forceSubwooferTrigger;

private final Debouncer shootDebounce = new Debouncer(0.075);

// FIXME: This is horrible and bad and terrible and get rid of it
private int lobbing = 0;

public ControlBoard(RobotContainer robot) {
this.robot = robot;

Expand All @@ -88,18 +78,6 @@ public ControlBoard(RobotContainer robot) {
driver.start.onFalling(() -> robot.drive.setRotation(new Rotation2d()));
driver.back.onFalling(() -> robot.drive.setRotation(new Rotation2d())); // Two buttons to reset gyro so the driver can't get confused

new Trigger(driver.leftBumper::isPressed)
.whileTrue(new AimTowardsLobCommand(robot.drive, robot.shooter))
.whileTrue(Commands.run(() -> robot.shooter.setTempAimCalculator(LobCalculator.INSTANCE)))
.onTrue(Commands.runOnce(() -> lobbing++))
.onFalse(Commands.runOnce(() -> lobbing--));

new Trigger(() -> driver.leftTrigger.isOutside(0.2))
.whileTrue(new AimTowardsLobCommand(robot.drive, robot.shooter))
.whileTrue(Commands.run(() -> robot.shooter.setTempAimCalculator(LowLobAimCalculator.INSTANCE)))
.onTrue(Commands.runOnce(() -> lobbing++))
.onFalse(Commands.runOnce(() -> lobbing--));

new Trigger(this::driverWantsAim).whileTrue(new AimTowardsSpeakerCommand(
robot.drive,
robot.shooter
Expand All @@ -123,11 +101,20 @@ public ControlBoard(RobotContainer robot) {
.onTrue(Commands.runOnce(() -> robot.intake.set(IntakeSubsystem.State.OFF)));


forceSubwooferTrigger = new Trigger(() -> driver.dpad.down.isPressed()).debounce(0.1);
forceSubwooferTrigger = new Trigger(() -> driver.dpad.down.isPressed()).debounce(0.1).whileTrue(robot.shooter.getSpeakerCommand());

new Trigger(operator.start::isPressed)
.onTrue(Commands.runOnce(robot.indexer::beginReverse))
.onFalse(Commands.runOnce(robot.indexer::endReverse));
.onFalse(Commands.runOnce(robot.indexer::endReverse))
.whileTrue(robot.shooter.getFeedCommand());

new Trigger(() -> driver.leftBumper.isPressed()).whileTrue(robot.shooter.getLobCommand()).whileTrue(new AimTowardsLobCommand(robot.drive, robot.shooter));

new Trigger(() -> operator.y.isPressed()).whileTrue(robot.shooter.getAmpCommand());

new Trigger(() -> operator.b.isPressed()).debounce(0.075).whileTrue(Commands.runOnce(() -> robot.indexer.setFeedToShooter(true))).onFalse(Commands.runOnce(() -> robot.indexer.setFeedToShooter(false)));

new Trigger(() -> operator.b.isPressed() && !driverWantsAim()).debounce(0.075).whileTrue(robot.shooter.getPoopCommand());

new Trigger(() -> CHARACTERISE_WHEEL_RADIUS.get()).whileTrue(new CharactarizeWheelCommand(robot.drive));
}
Expand Down Expand Up @@ -192,8 +179,6 @@ private enum ClimbState {

@Override
public void periodic() {
if (DriverStation.isDisabled())
lobbing = 0;

if (!DriverStation.isTeleop()) {
climbState = ClimbState.IDLE;
Expand Down Expand Up @@ -248,10 +233,6 @@ public void periodic() {
chassisRequest,
DriveRequestType.Velocity);

// Indexer uses the intake state also
boolean operatorWantsShoot = shootDebounce.calculate(operator.b.isPressed());
robot.indexer.setFeedToShooter(operatorWantsShoot);

boolean climbToggle = operator.rightBumper.isRising();
boolean climbCancel = operator.leftBumper.isRising();

Expand Down Expand Up @@ -293,18 +274,14 @@ public void periodic() {

robot.intake.setReverse(operator.back.isPressed());

// boolean shootAmp = operator.y.isPressed();
// if (shootAmp)
// robot.shooter.setTempAimCalculator(AmpAimCalculator.INSTANCE);

// boolean shootManual = operator.x.isPressed();
// if (shootManual)
// robot.shooter.setTempAimCalculator(ManualAimCalculator.INSTANCE);

// ShooterSubsystem.FlywheelControl flywheelControl = ShooterSubsystem.FlywheelControl.IDLE;
// if (operator.start.isPressed())
// flywheelControl = ShooterSubsystem.FlywheelControl.REVERSE;
// else if (driverWantsAim() || shootAmp || shootManual || forceToSubwoofer || lobbing != 0)
// flywheelControl = ShooterSubsystem.FlywheelControl.SHOOT;

// else if (operatorWantsShoot)
// flywheelControl = ShooterSubsystem.FlywheelControl.POOP;
// robot.shooter.setFlywheelControl(flywheelControl);
Expand Down
185 changes: 111 additions & 74 deletions Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/ShooterSubsystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,6 +10,7 @@
import com.swrobotics.robot.logging.FieldView;
import com.swrobotics.robot.logging.SimView;
import com.swrobotics.robot.subsystems.speaker.aim.AimCalculator;
import com.swrobotics.robot.subsystems.speaker.aim.AmpAimCalculator;
import com.swrobotics.robot.subsystems.speaker.aim.LobCalculator;
import com.swrobotics.robot.subsystems.speaker.aim.TableAimCalculator;
import com.swrobotics.robot.subsystems.swerve.SwerveDrive;
Expand Down Expand Up @@ -69,7 +70,7 @@ public ShooterSubsystem(SwerveDrive drive, IndexerSubsystem indexer) {

flywheelControl = FlywheelControl.SHOOT;

setDefaultCommand(idle().ignoringDisable(true));
setDefaultCommand(getIdleCommand().ignoringDisable(true));
handleIdle();
}

Expand All @@ -96,41 +97,46 @@ public void periodic() {
// // Use the selected aim calculator
// AimCalculator.Aim aim;
// if (aimCalculator instanceof LobCalculator) {
// double distToLob = getLobZonePosition().getDistance(drive.getEstimatedPose().getTranslation());
// Pose2d robotPose = drive.getEstimatedPose();
// Translation2d robotPos = robotPose.getTranslation();
// ChassisSpeeds robotSpeeds = drive.getFieldRelativeSpeeds();
// double distToLob =
// getLobZonePosition().getDistance(drive.getEstimatedPose().getTranslation());
// Pose2d robotPose = drive.getEstimatedPose();
// Translation2d robotPos = robotPose.getTranslation();
// ChassisSpeeds robotSpeeds = drive.getFieldRelativeSpeeds();

// Translation2d target = getLobZonePosition();
// Rotation2d angleToTarget = target.minus(robotPos).getAngle();
// Translation2d target = getLobZonePosition();
// Rotation2d angleToTarget = target.minus(robotPos).getAngle();

// // Relative to the target
// Translation2d robotVelocity = new Translation2d(robotSpeeds.vxMetersPerSecond,
// robotSpeeds.vyMetersPerSecond).rotateBy(angleToTarget);
// aim = LobCalculator.INSTANCE.calculateAim(distToLob, robotVelocity.getX());
// // Relative to the target
// Translation2d robotVelocity = new
// Translation2d(robotSpeeds.vxMetersPerSecond,
// robotSpeeds.vyMetersPerSecond).rotateBy(angleToTarget);
// aim = LobCalculator.INSTANCE.calculateAim(distToLob, robotVelocity.getX());

// } else {
// double distToSpeaker = getSpeakerPosition().getDistance(drive.getEstimatedPose().getTranslation());
// Pose2d robotPose = drive.getEstimatedPose();
// Translation2d robotPos = robotPose.getTranslation();
// ChassisSpeeds robotSpeeds = drive.getFieldRelativeSpeeds();

// Translation2d target = getSpeakerPosition();
// Rotation2d angleToTarget = target.minus(robotPos).getAngle();

// // Relative to the target
// Translation2d robotVelocity = new Translation2d(robotSpeeds.vxMetersPerSecond,
// robotSpeeds.vyMetersPerSecond).rotateBy(angleToTarget);
// if (aimCalculator instanceof TableAimCalculator)
// aim = ((TableAimCalculator) aimCalculator).calculateAim(distToSpeaker, robotVelocity.getX());
// else
// aim = aimCalculator.calculateAim(distToSpeaker);

// if (RobotBase.isSimulation())
// SimView.lobTrajectory.update(
// aim.flywheelVelocity() / NTData.SHOOTER_LOB_POWER_COEFFICIENT.get(),
// aim.pivotAngle());
// SimView.lobTrajectory.clear();
// double distToSpeaker =
// getSpeakerPosition().getDistance(drive.getEstimatedPose().getTranslation());
// Pose2d robotPose = drive.getEstimatedPose();
// Translation2d robotPos = robotPose.getTranslation();
// ChassisSpeeds robotSpeeds = drive.getFieldRelativeSpeeds();

// Translation2d target = getSpeakerPosition();
// Rotation2d angleToTarget = target.minus(robotPos).getAngle();

// // Relative to the target
// Translation2d robotVelocity = new
// Translation2d(robotSpeeds.vxMetersPerSecond,
// robotSpeeds.vyMetersPerSecond).rotateBy(angleToTarget);
// if (aimCalculator instanceof TableAimCalculator)
// aim = ((TableAimCalculator) aimCalculator).calculateAim(distToSpeaker,
// robotVelocity.getX());
// else
// aim = aimCalculator.calculateAim(distToSpeaker);

// if (RobotBase.isSimulation())
// SimView.lobTrajectory.update(
// aim.flywheelVelocity() / NTData.SHOOTER_LOB_POWER_COEFFICIENT.get(),
// aim.pivotAngle());
// SimView.lobTrajectory.clear();
// }
// targetAim = aim;
// aimCalculator = tableAimCalculator;
Expand All @@ -140,39 +146,40 @@ public void periodic() {

// isPreparing = false;
// if (DriverStation.isDisabled() || !pivot.hasCalibrated())
// return;
// return;

// if (DriverStation.isAutonomous() && (aim != null)) {
// // Have the shooter be constantly active during auto
// isPreparing = true;
// flywheel.setTargetVelocity(aim.flywheelVelocity());
// pivot.setTargetAngle(aim.pivotAngle() / MathUtil.TAU);
// // Have the shooter be constantly active during auto
// isPreparing = true;
// flywheel.setTargetVelocity(aim.flywheelVelocity());
// pivot.setTargetAngle(aim.pivotAngle() / MathUtil.TAU);
// } else if (flywheelControl == FlywheelControl.REVERSE) {
// pivot.setTargetAngle(NTData.SHOOTER_PIVOT_REVERSE_ANGLE.get() / 360.0);
// flywheel.setDutyCycle(-NTData.SHOOTER_FLYWHEEL_REVERSE_SPEED.get());
// } else if (flywheelControl == FlywheelControl.SHOOT || afterShootDelay.calculate(indexer.hasPiece())) {
// if (aim != null) {
// isPreparing = true;
// if (flywheelControl == FlywheelControl.SHOOT)
// flywheel.setTargetVelocity(aim.flywheelVelocity());
// else if (flywheelControl == FlywheelControl.POOP)
// flywheel.setDutyCycle(NTData.SHOOTER_FLYWHEEL_POOP_SPEED.get());
// else
// flywheel.setDutyCycle(NTData.SHOOTER_FLYWHEEL_IDLE_SPEED.get());
// pivot.setTargetAngle(aim.pivotAngle() / MathUtil.TAU);
// } else {
// if (flywheelControl == FlywheelControl.POOP)
// flywheel.setDutyCycle(NTData.SHOOTER_FLYWHEEL_POOP_SPEED.get());
// else
// flywheel.setDutyCycle(NTData.SHOOTER_FLYWHEEL_IDLE_SPEED.get());
// pivot.setIdle();
// }
// pivot.setTargetAngle(NTData.SHOOTER_PIVOT_REVERSE_ANGLE.get() / 360.0);
// flywheel.setDutyCycle(-NTData.SHOOTER_FLYWHEEL_REVERSE_SPEED.get());
// } else if (flywheelControl == FlywheelControl.SHOOT ||
// afterShootDelay.calculate(indexer.hasPiece())) {
// if (aim != null) {
// isPreparing = true;
// if (flywheelControl == FlywheelControl.SHOOT)
// flywheel.setTargetVelocity(aim.flywheelVelocity());
// else if (flywheelControl == FlywheelControl.POOP)
// flywheel.setDutyCycle(NTData.SHOOTER_FLYWHEEL_POOP_SPEED.get());
// else
// flywheel.setDutyCycle(NTData.SHOOTER_FLYWHEEL_IDLE_SPEED.get());
// pivot.setTargetAngle(aim.pivotAngle() / MathUtil.TAU);
// } else {
// if (flywheelControl == FlywheelControl.POOP)
// flywheel.setDutyCycle(NTData.SHOOTER_FLYWHEEL_POOP_SPEED.get());
// else
// flywheel.setDutyCycle(NTData.SHOOTER_FLYWHEEL_IDLE_SPEED.get());
// pivot.setIdle();
// }
// } else if (flywheelControl == FlywheelControl.POOP) {
// flywheel.setDutyCycle(NTData.SHOOTER_FLYWHEEL_POOP_SPEED.get());
// pivot.setNeutral();
// flywheel.setDutyCycle(NTData.SHOOTER_FLYWHEEL_POOP_SPEED.get());
// pivot.setNeutral();
// } else {
// flywheel.setNeutral();
// pivot.setNeutral();
// flywheel.setNeutral();
// pivot.setNeutral();
// }

NTData.SHOOTER_READY.set(isReadyToShoot());
Expand All @@ -183,19 +190,19 @@ public void periodic() {
}

private void handleSpeaker() {
double distToSpeaker = getSpeakerPosition().getDistance(drive.getEstimatedPose().getTranslation());
Pose2d robotPose = drive.getEstimatedPose();
Translation2d robotPos = robotPose.getTranslation();
ChassisSpeeds robotSpeeds = drive.getFieldRelativeSpeeds();

Translation2d target = getSpeakerPosition();
Rotation2d angleToTarget = target.minus(robotPos).getAngle();

// Relative to the target
Translation2d robotVelocity = new Translation2d(robotSpeeds.vxMetersPerSecond,
robotSpeeds.vyMetersPerSecond).rotateBy(angleToTarget);
AimCalculator.Aim aim = TableAimCalculator.INSTANCE.calculateAim(distToSpeaker, robotVelocity.getX());
if (/*afterShootDelay.calculate(indexer.hasPiece()) && */(aim != null)) {
double distToSpeaker = getSpeakerPosition().getDistance(drive.getEstimatedPose().getTranslation());
Pose2d robotPose = drive.getEstimatedPose();
Translation2d robotPos = robotPose.getTranslation();
ChassisSpeeds robotSpeeds = drive.getFieldRelativeSpeeds();

Translation2d target = getSpeakerPosition();
Rotation2d angleToTarget = target.minus(robotPos).getAngle();

// Relative to the target
Translation2d robotVelocity = new Translation2d(robotSpeeds.vxMetersPerSecond,
robotSpeeds.vyMetersPerSecond).rotateBy(angleToTarget);
AimCalculator.Aim aim = TableAimCalculator.INSTANCE.calculateAim(distToSpeaker, robotVelocity.getX());
if (/* afterShootDelay.calculate(indexer.hasPiece()) && */(aim != null)) {
pivot.setTargetAngle(aim.pivotAngle() / MathUtil.TAU);
flywheel.setTargetVelocity(aim.flywheelVelocity());
targetAim = aim;
Expand All @@ -214,10 +221,40 @@ private void handleIdle() {
targetAim = new AimCalculator.Aim(10.0, 22 / 360.0, 0);
}

public Command idle() {
public Command getIdleCommand() {
return Commands.run(() -> handleIdle(), this).withName("Shooter Idle");
}

private void handleAmp() {

}

public Command getAmpCommand() {
return Commands.run(() -> handleAmp(), this).withName("Shooter Amp");
}

private void handleLob() {

}

public Command getLobCommand() {
return Commands.run(() -> handleLob(), this).withName("Shooter Lob");
}

private void handleFeed() {
pivot.setTargetAngle(NTData.SHOOTER_PIVOT_REVERSE_ANGLE.get() / 360.0);
flywheel.setDutyCycle(-NTData.SHOOTER_FLYWHEEL_REVERSE_SPEED.get());
}

public Command getFeedCommand() {
return Commands.run(() -> handleFeed(), this).withName("Shooter Feed");
}

public Command getPoopCommand() {
return Commands.run(() -> flywheel.setDutyCycle(NTData.SHOOTER_FLYWHEEL_POOP_SPEED.get()), this)
.withName("Shooter Poop");
}

NTDouble pctErr = new NTDouble("Shooter/Debug/Percent Error", 0);

@Override
Expand Down

0 comments on commit fe0b139

Please sign in to comment.