diff --git a/src/main/java/frc/robot/commands/ShooterCalc.java b/src/main/java/frc/robot/commands/ShooterCalc.java index df9133f0..b28f61a8 100644 --- a/src/main/java/frc/robot/commands/ShooterCalc.java +++ b/src/main/java/frc/robot/commands/ShooterCalc.java @@ -1,7 +1,3 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - package frc.robot.commands; import java.util.Optional; import java.util.function.BooleanSupplier; @@ -11,27 +7,51 @@ import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.subsystems.Swerve; import frc.robot.subsystems.shooter.*; import frc.robot.util.Constants.FieldConstants; import frc.robot.util.Constants.ShooterConstants; import frc.robot.util.SpeedAnglePair; -public class ShooterCalc extends Command { +public class ShooterCalc { + + private Pivot pivot; + private Shooter shooter; + private BooleanSupplier stopAiming; - public Command prepareShooter(BooleanSupplier supp, Pose2d robotPose, Pivot pivot, Shooter shooter) { - SpeedAnglePair pair = (supp.getAsBoolean()) - ? getSpeaker(robotPose) - : getAmp(robotPose); + public ShooterCalc() { + this.pivot = new Pivot(); + this.shooter = new Shooter(); + this.stopAiming = (() -> false); + } + + public Command prepareFireCommand(BooleanSupplier shootAtSpeaker, Pose2d robotPose) { + SpeedAnglePair pair = calculateSpeed(robotPose, shootAtSpeaker.getAsBoolean()); return Commands.runOnce(() -> pivot.setAngle(pair.getAngle())) .alongWith(Commands.runOnce(() -> shooter.setSpeed(pair.getSpeed()))); } - private SpeedAnglePair getSpeaker(Pose2d robotPose) { - return calculateSpeed(robotPose, true); + public Command prepareFireMovingCommand(BooleanSupplier shootAtSpeaker, Swerve swerve) { + if (stopAiming.getAsBoolean()) { + return Commands.runOnce(() -> toggleStopAiming()); + } + return Commands.runOnce(() -> toggleStopAiming()) + .andThen(prepareFireCommand(shootAtSpeaker, swerve.getPose())).repeatedly() + .until(stopAiming); + } + + public Command prepareShooterCommand(BooleanSupplier shootAtSpeaker, Pose2d robotPose) { + SpeedAnglePair pair = calculateSpeed(robotPose, shootAtSpeaker.getAsBoolean()); + return Commands.runOnce(() -> shooter.setSpeed(pair.getSpeed())); + } + + public Command preparePivotCommand(BooleanSupplier shootAtSpeaker, Pose2d robotPose) { + SpeedAnglePair pair = calculateSpeed(robotPose, shootAtSpeaker.getAsBoolean()); + return Commands.runOnce(() -> pivot.setAngle(pair.getAngle())); } - private SpeedAnglePair getAmp(Pose2d robotPose) { - return calculateSpeed(robotPose, false); + private void toggleStopAiming() { + this.stopAiming = (() -> !stopAiming.getAsBoolean()); } private SpeedAnglePair calculateSpeed(Pose2d robotPose, boolean shootingAtSpeaker) { @@ -40,7 +60,7 @@ private SpeedAnglePair calculateSpeed(Pose2d robotPose, boolean shootingAtSpeake int positionIndex = FieldConstants.ALLIANCE == Optional.ofNullable(Alliance.Blue) ? 0 : 1; // Get our position relative to the desired field element - if (shootingAtSpeaker) { + if (shootingAtSpeaker) { robotPose.relativeTo(FieldConstants.SPEAKER_POSITIONS[positionIndex]); } else { robotPose.relativeTo(FieldConstants.AMP_POSITIONS[positionIndex]); @@ -55,4 +75,4 @@ private SpeedAnglePair calculateSpeed(Pose2d robotPose, boolean shootingAtSpeake return lowerPair.interpolate(upperPair, (distance - lowerIndex) / (upperIndex - lowerIndex)); } -} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java deleted file mode 100644 index 23f01d71..00000000 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ /dev/null @@ -1,84 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.subsystems; - -import java.util.Optional; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.util.Neo; -import frc.robot.util.SpeedAnglePair; -import frc.robot.util.Constants.FieldConstants; -import frc.robot.util.Constants.ShooterConstants; - -public class Shooter extends SubsystemBase { - /** Creates a new shooter. */ - private final Neo motorLeft; - private final Neo motorRight; - private final Neo pivotMotor; - - public Shooter() { - - motorLeft = new Neo(ShooterConstants.LEFT_SHOOTER_CAN_ID); - motorRight = new Neo(ShooterConstants.RIGHT_SHOOTER_CAN_ID); - - pivotMotor = new Neo(ShooterConstants.SHOOTER_PIVOT_CAN_ID); - - configMotors(); - - } - - public void configMotors() { - motorLeft.setPID( - ShooterConstants.SHOOTER_P, - ShooterConstants.SHOOTER_I, - ShooterConstants.SHOOTER_D, - ShooterConstants.SHOOTER_MIN_OUTPUT, - ShooterConstants.SHOOTER_MAX_OUTPUT); - - motorLeft.addFollower(motorRight, true); - // Current limit - // Velocity conversion factor (make gear ratio in constants) - - } - - @Override - public void periodic() { - // This method will be called once per scheduler run - } - - public Command shoot(Pose2d position, boolean shootingAtSpeaker) { - SpeedAnglePair pair = calculateSpeed(position, shootingAtSpeaker); - return runOnce(() -> motorLeft.setTargetVelocity(1)); - } - - public Command stop() { - return runOnce(() -> motorLeft.setTargetVelocity(0)); - } - - public SpeedAnglePair calculateSpeed(Pose2d robotPose, boolean shootingAtSpeaker) { - // Constants have blue alliance positions at index 0 - // and red alliance positions at index 1 - int positionIndex = FieldConstants.ALLIANCE == Optional.ofNullable(Alliance.Blue) ? 0 : 1; - - // Get our position relative to the desired field element - if (shootingAtSpeaker) { - robotPose.relativeTo(FieldConstants.SPEAKER_POSITIONS[positionIndex]); - } else { - robotPose.relativeTo(FieldConstants.AMP_POSITIONS[positionIndex]); - } - - double distance = Units.metersToFeet(robotPose.getTranslation().getNorm()); - int lowerIndex = (int) (Math.floor(distance / ShooterConstants.MEASUREMENT_INTERVAL_FEET) * ShooterConstants.MEASUREMENT_INTERVAL_FEET); - int upperIndex = (int) (Math.ceil(distance / ShooterConstants.MEASUREMENT_INTERVAL_FEET) * ShooterConstants.MEASUREMENT_INTERVAL_FEET); - - SpeedAnglePair lowerPair = ShooterConstants.SPEAKER_DISTANCES_TO_SPEEDS_AND_ANGLE_MAP.get(lowerIndex); - SpeedAnglePair upperPair = ShooterConstants.SPEAKER_DISTANCES_TO_SPEEDS_AND_ANGLE_MAP.get(upperIndex); - - return lowerPair.interpolate(upperPair, (distance - lowerIndex) / (upperIndex - lowerIndex)); - } -} diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index c7763148..bba33ee8 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -40,8 +40,8 @@ public void periodic() { // This method will be called once per scheduler run } - public void setSpeed(double calc) { - motorLeft.setTargetVelocity(calc); + public void setSpeed(double speed) { + motorLeft.setTargetVelocity(speed); } public void stop() {