diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 1501f3f..bde37f8 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -339,6 +339,7 @@ public static class FeederConstants { public static final int feederNoteSensorID = 0; public static final double FeederMotorSpeed = 0.8; public static final double getNoteSpeed = 0; + public static final double TimeToFeed = 0.7; } diff --git a/src/main/java/frc/robot/aRobotOperations/PrepareForShoot.java b/src/main/java/frc/robot/aRobotOperations/PrepareForShoot.java index 416a2fd..b3c9073 100644 --- a/src/main/java/frc/robot/aRobotOperations/PrepareForShoot.java +++ b/src/main/java/frc/robot/aRobotOperations/PrepareForShoot.java @@ -6,7 +6,7 @@ import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import frc.robot.basicCommands.ShooterArmCommands.ShooterArmSpeakerAngle; -import frc.robot.basicCommands.ShooterCommands.ShooterSpeakerSetSpeed; +import frc.robot.basicCommands.ShooterCommands.ShooterSpeaker; public class PrepareForShoot extends ParallelCommandGroup { /** Creates a new PrepareForShoot. */ @@ -14,7 +14,7 @@ public PrepareForShoot() { addCommands( new ShooterArmSpeakerAngle(), - new ShooterSpeakerSetSpeed() + new ShooterSpeaker() ); } } diff --git a/src/main/java/frc/robot/aRobotOperations/ShootSpeaker.java b/src/main/java/frc/robot/aRobotOperations/ShootSpeaker.java index b7d8077..f537894 100644 --- a/src/main/java/frc/robot/aRobotOperations/ShootSpeaker.java +++ b/src/main/java/frc/robot/aRobotOperations/ShootSpeaker.java @@ -4,10 +4,10 @@ package frc.robot.aRobotOperations; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; import frc.robot.basicCommands.feederCommands.FeedToShooter; -public class ShootSpeaker extends ParallelCommandGroup { +public class ShootSpeaker extends ParallelRaceGroup { /** Creates a new ShootSpeaker. */ public ShootSpeaker() { addCommands( diff --git a/src/main/java/frc/robot/basicCommands/ShooterArmCommands/ShooterArmSpeakerAngle.java b/src/main/java/frc/robot/basicCommands/ShooterArmCommands/ShooterArmSpeakerAngle.java index 9d0e9f4..3e04aec 100644 --- a/src/main/java/frc/robot/basicCommands/ShooterArmCommands/ShooterArmSpeakerAngle.java +++ b/src/main/java/frc/robot/basicCommands/ShooterArmCommands/ShooterArmSpeakerAngle.java @@ -5,12 +5,12 @@ package frc.robot.basicCommands.ShooterArmCommands; import edu.wpi.first.wpilibj2.command.Command; -import frc.Utils.vision.Vision; import frc.robot.subsystems.ShooterArmSubsystem; +import frc.robot.subsystems.SwerveSubsystem; public class ShooterArmSpeakerAngle extends Command { private final ShooterArmSubsystem shooterArmSubsystem = ShooterArmSubsystem.getInstance(); - private final Vision vision = Vision.getInstance(); + private final SwerveSubsystem swerve = SwerveSubsystem.getInstance(); /** Creates a new ShooterAngleFromDistanceInterpolation. */ public ShooterArmSpeakerAngle() { @@ -25,7 +25,7 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - shooterArmSubsystem.moveArmTo(shooterArmSubsystem.angleFromDistance(vision.DistanceFromTarget())); + shooterArmSubsystem.moveArmTo(shooterArmSubsystem.angleFromDistance(swerve.getPose())); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/basicCommands/ShooterCommands/ShooterSpeakerSetSpeed.java b/src/main/java/frc/robot/basicCommands/ShooterCommands/ShooterSpeaker.java similarity index 91% rename from src/main/java/frc/robot/basicCommands/ShooterCommands/ShooterSpeakerSetSpeed.java rename to src/main/java/frc/robot/basicCommands/ShooterCommands/ShooterSpeaker.java index 67e3ac3..1066272 100644 --- a/src/main/java/frc/robot/basicCommands/ShooterCommands/ShooterSpeakerSetSpeed.java +++ b/src/main/java/frc/robot/basicCommands/ShooterCommands/ShooterSpeaker.java @@ -8,12 +8,12 @@ import frc.robot.subsystems.ShooterSubsysem; import frc.robot.subsystems.SwerveSubsystem; -public class ShooterSpeakerSetSpeed extends Command { +public class ShooterSpeaker extends Command { private final ShooterSubsysem shooterSubsystem = ShooterSubsysem.getInstance(); private final SwerveSubsystem swerve = SwerveSubsystem.getInstance(); /** Creates a new ShooterSetSpeedInterpolation. */ - public ShooterSpeakerSetSpeed() { + public ShooterSpeaker() { this.addRequirements(shooterSubsystem); } @@ -31,6 +31,7 @@ public void execute() { // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { + shooterSubsystem.coast(); } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/basicCommands/feederCommands/FeedToShooter.java b/src/main/java/frc/robot/basicCommands/feederCommands/FeedToShooter.java index 0f66814..d39f0e8 100644 --- a/src/main/java/frc/robot/basicCommands/feederCommands/FeedToShooter.java +++ b/src/main/java/frc/robot/basicCommands/feederCommands/FeedToShooter.java @@ -4,16 +4,20 @@ package frc.robot.basicCommands.feederCommands; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants.FeederConstants; import frc.robot.subsystems.FeederSubsystem; import frc.robot.subsystems.ShooterArmSubsystem; import frc.robot.subsystems.ShooterSubsysem; import static frc.robot.Constants.FeederConstants.*; public class FeedToShooter extends Command { - private final ShooterSubsysem shooterSubsysem = ShooterSubsysem.getInstance(); + private final ShooterSubsysem shooterSubsystem = ShooterSubsysem.getInstance(); private final ShooterArmSubsystem shooterArmSubsystem = ShooterArmSubsystem.getInstance(); private final FeederSubsystem feederSubsystem = FeederSubsystem.getInstance(); + Timer timer = new Timer(); + boolean startedShooting = false; /** Creates a new StartFeeder. */ public FeedToShooter() { @@ -29,8 +33,10 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if (shooterSubsysem.checkIfShooterAtSpeed() && shooterArmSubsystem.isArmReady()) { + if (!startedShooting && shooterSubsystem.checkIfShooterAtSpeed() && shooterArmSubsystem.isArmReady()) { feederSubsystem.setSpeed(FeederShootSpeed); + startedShooting = true; + timer.restart(); } } @@ -43,6 +49,6 @@ public void end(boolean interrupted) { // Returns true when the command should end. @Override public boolean isFinished() { - return false; + return timer.hasElapsed(FeederConstants.TimeToFeed); } } diff --git a/src/main/java/frc/robot/subsystems/ShooterArmSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterArmSubsystem.java index 61552c0..5ba7bf9 100644 --- a/src/main/java/frc/robot/subsystems/ShooterArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterArmSubsystem.java @@ -9,9 +9,12 @@ import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.MotionMagicVoltage; import com.ctre.phoenix6.hardware.TalonFX; + +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.Utils.interpolation.InterpolateUtil; +import frc.Utils.vision.Vision; import frc.robot.Constants; import static frc.robot.Constants.ShooterArmConstants.*; @@ -25,6 +28,7 @@ public class ShooterArmSubsystem extends SubsystemBase { private TalonFX m_shooterArmMotor; private final MotionMagicVoltage motionMagic = new MotionMagicVoltage(shooterArmStartPose); DigitalInput limitSwitch; + private final Vision vision = Vision.getInstance(); // the instance private static ShooterArmSubsystem instance; @@ -109,8 +113,8 @@ public void moveArmBySpeed(DoubleSupplier speed) { m_shooterArmMotor.set(speed.getAsDouble()); } - public double angleFromDistance(double distance) { - return InterpolateUtil.interpolate(SHOOTER_ANGLE_INTERPOLATION_MAP, distance); + public double angleFromDistance(Pose2d pose) { + return InterpolateUtil.interpolate(SHOOTER_ANGLE_INTERPOLATION_MAP, vision.DistanceFromTarget(pose)); } @Override diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsysem.java b/src/main/java/frc/robot/subsystems/ShooterSubsysem.java index 8985468..b7ae53b 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsysem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsysem.java @@ -8,7 +8,10 @@ import com.ctre.phoenix6.configs.MotionMagicConfigs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.MotionMagicVelocityVoltage; +import com.ctre.phoenix6.controls.NeutralOut; import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.NeutralModeValue; + import static frc.robot.Constants.ShooterConstants.*; import edu.wpi.first.math.geometry.Pose2d; @@ -25,6 +28,8 @@ public class ShooterSubsysem extends SubsystemBase { private static ShooterSubsysem instance; + NeutralOut neutralOut = new NeutralOut(); + public static ShooterSubsysem getInstance() { if (instance == null) { instance = new ShooterSubsysem(); @@ -58,6 +63,8 @@ private ShooterSubsysem() { configs.Voltage.PeakReverseVoltage = PeakReverseVoltage; configs.Feedback.SensorToMechanismRatio = SensorToMechanismRatio; + + m_shooterMotor.setNeutralMode(NeutralModeValue.Coast); // Checking if m_shooterMotor apply configs StatusCode status = StatusCode.StatusCodeNotInitialized; @@ -88,6 +95,10 @@ public double speakerInterpolate(Pose2d pose2d) { return InterpolateUtil.interpolate(ShooterInterpolation, vision.DistanceFromTarget(pose2d)); } + public void coast() { + m_shooterMotor.setControl(neutralOut); + } + @Override public void periodic() { // This method will be called once per scheduler run