diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 6fad0ca..5a49a3e 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -99,7 +99,7 @@ public class RobotContainer { ); private Command RobotShoot = new SequentialCommandGroup( - new InstantCommand(() -> m_robotShooter.setShooterReady()), + new InstantCommand(() -> m_robotShooter.spinUpShooting()), new RunCommand( () -> { m_robotSwerveDrive.driveFacingPosition( @@ -110,9 +110,9 @@ public class RobotContainer { }, m_robotSwerveDrive), new InstantCommand(()->m_robotIntake.setMode(IntakeMode.Idle)), new WaitCommand(5), - new InstantCommand(()->m_robotShooter.setShooterShoot()), + new InstantCommand(()->m_robotShooter.allowShooting()), new WaitCommand(10), - new InstantCommand(()->m_robotShooter.setShooterNOTShoot()) + new InstantCommand(()->m_robotShooter.denyShooting()) ); // private Command RobotShoot = new SequentialCommandGroup( @@ -256,18 +256,18 @@ public class RobotContainer { new Trigger(() -> getDeadbandedOperatorController().getLeftTriggerAxis() >= 0.5) .onTrue(new InstantCommand(() -> { - m_robotShooter.setShooterNotReady(); + m_robotShooter.spinUpIdle(); })); new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON) .onTrue(new InstantCommand(() -> { - m_robotShooter.setShooterReady(); + m_robotShooter.spinUpShooting(); m_robotIntake.setMode(IntakeMode.Idle); })); new JoystickButton(getDeadbandedOperatorController(), XboxController.BACK_BUTTON) .onTrue(new InstantCommand(() -> { - m_robotShooter.setShooterReadyFeeder(); + m_robotShooter.spinUpFeeding(); })); @@ -275,9 +275,9 @@ public class RobotContainer { new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON) .onTrue(new InstantCommand(() -> { - m_robotShooter.setShooterShoot(); + m_robotShooter.allowShooting(); })).onFalse(new InstantCommand(() -> { - m_robotShooter.setShooterNOTShoot(); + m_robotShooter.denyShooting(); })); new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) diff --git a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java index a92d5f8..4ea2aaa 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java @@ -1,5 +1,6 @@ package frc4388.robot.subsystems.shooter; +import static edu.wpi.first.units.Units.Amps; import static edu.wpi.first.units.Units.Rotation; import static edu.wpi.first.units.Units.RotationsPerSecond; @@ -7,8 +8,10 @@ import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.constants.Constants; import frc4388.robot.subsystems.LED; @@ -59,24 +62,24 @@ public class Shooter extends SubsystemBase { private ShooterMode mode = ShooterMode.Idle; private boolean shooterButtonReady = false; - public void setShooterShooting() { + public void spinUpShooting() { this.mode = ShooterMode.Shooting; } - public void setShooterFeeding() { + public void spinUpFeeding() { this.mode = ShooterMode.Feeding; } - public void setShooterIdle() { + public void spinUpIdle() { this.mode = ShooterMode.Idle; } - public void setShooterShoot() { + public void allowShooting() { shooterButtonReady = true; } - public void setShooterNOTShoot() { + public void denyShooting() { shooterButtonReady = false; } @@ -95,18 +98,30 @@ public class Shooter extends SubsystemBase { io.updateInputs(state); - ChassisSpeeds speed = m_SwerveDrive.chassisSpeeds; - double XYSpeed = Math.sqrt(Math.pow(speed.vxMetersPerSecond,2) + Math.pow(speed.vyMetersPerSecond,2)); - double AngSpeed = Math.abs(speed.omegaRadiansPerSecond * (180/Math.PI)); + // Get robot positon and speeds + ChassisSpeeds chassisSpeeds = m_SwerveDrive.chassisSpeeds; + double XYSpeed = Math.sqrt(Math.pow(chassisSpeeds.vxMetersPerSecond,2) + Math.pow(chassisSpeeds.vyMetersPerSecond,2)); + double AngSpeed = Math.abs(chassisSpeeds.omegaRadiansPerSecond * (180/Math.PI)); Pose2d robotPose2d = m_SwerveDrive.getPose2d(); - // - double distanceToHub = (robotPose2d.getTranslation().minus(FieldPositions.HUB_POSITION).getNorm()); + + + // Calculate aim lead + // Get the current speed of the robot + Translation2d robotSpeed = new Translation2d( + chassisSpeeds.vxMetersPerSecond, + chassisSpeeds.vyMetersPerSecond + ); + + // Calculate a point to aim ahead of the actual position. + Translation2d fieldPosLead = robotSpeed.times(ShooterConstants.AIM_LEAD_TIME.get()).plus(robotPose2d.getTranslation()); + + // Get the robot's aim distance to hub + double distanceToHub = (fieldPosLead.minus(FieldPositions.HUB_POSITION).getNorm()); + //Center of hub to cameras in inches Logger.recordOutput("Hub Dist", distanceToHub); - - boolean driverError = // XYSpeed <= ShooterConstants.ROBOT_SPEED_TOLERANCE.get() | // AngSpeed <= ShooterConstants.ROBOT_ANG_SPEED_TOLERANCE.get() | @@ -199,7 +214,13 @@ public class Shooter extends SubsystemBase { break; case Idle: - io.setShooterVelocity(state, RotationsPerSecond.of(ShooterConstants.SHOOTER_RESTING_VELOCITY.get())); + + io.setShooterCurrentLimitSpeed( + state, + ShooterConstants.SHOOTER_IDLE_PERCENT_OUTPUT.get(), + Amps.of(ShooterConstants.SHOOTER_IDLE_MAX_CURRENT.get()), + RotationsPerSecond.of(ShooterConstants.INDEXER_REVERSE_OUTPUT.get()) + ); io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get()); m_robotLED.setMode(Constants.LEDConstants.DEFAULT_PATTERN); break; diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java index 3f4c63d..89b3cb7 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java @@ -18,14 +18,18 @@ public class ShooterConstants { public static final double SHOOTERMOTOR_GEAR_RATIO = 1.5; public static final double INDEXER_GEAR_RATIO = 1.; - // public static final ConfigurableDouble SHOOTER_ACTIVE_VELOCITY = new ConfigurableDouble("Shooter Active Velocity", -40); public static final ConfigurableDouble SHOOTER_MAX_VELOCITY = new ConfigurableDouble("Shooter MAX Velocity", 60); - public static final ConfigurableDouble SHOOTER_RESTING_VELOCITY = new ConfigurableDouble("Shooter Resting Velocity", 0.0); public static final ConfigurableDouble SHOOTER_FEED_VELOCITY = new ConfigurableDouble("Shooter Feed Velocity", -35); + // public static final ConfigurableDouble SHOOTER_RESTING_VELOCITY = new ConfigurableDouble("Shooter Resting Velocity", 0.0); + public static final ConfigurableDouble SHOOTER_IDLE_PERCENT_OUTPUT = new ConfigurableDouble("Shooter idle % output", 0.3); + public static final ConfigurableDouble SHOOTER_IDLE_TARGET_VEL = new ConfigurableDouble("Shooter idle target velocity", 20.); + public static final ConfigurableDouble SHOOTER_IDLE_MAX_CURRENT = new ConfigurableDouble("Shooter Idle max current", 10); + public static final ConfigurableDouble INDEXER_FORWARD_OUTPUT = new ConfigurableDouble("Indexer FWD % Output", -0.4); public static final ConfigurableDouble INDEXER_REVERSE_OUTPUT = new ConfigurableDouble("Indexer reverse % Output", 0.0); + public static final ConfigurableDouble AIM_LEAD_TIME = new ConfigurableDouble("Aim lead time", 0); // Shoot mode tolerances diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java index 463cb4b..4fd3916 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java @@ -38,6 +38,12 @@ public interface ShooterIO { // public default void setShooterAngle(ShooterState state, Angle angle) {} // public default void setShooterPitch(ShooterState state, Angle angle) {} public default void setShooterVelocity(ShooterState state, AngularVelocity angularVelocity) {} + public default void setShooterCurrentLimitSpeed( + ShooterState state, + double percentOutput, + Current currentLimit, + AngularVelocity targetVelocity + ) {} // public default void setMotor2Velocity(ShooterState state, AngularVelocity angularVelocity) {} public default void setIndexerOutput(ShooterState state, double percentOutput) {} diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java index 04fb459..6b14722 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java @@ -1,9 +1,13 @@ package frc4388.robot.subsystems.shooter; +import static edu.wpi.first.units.Units.Amps; +import static edu.wpi.first.units.Units.RotationsPerSecond; + import com.ctre.phoenix6.controls.VelocityDutyCycle; import com.ctre.phoenix6.hardware.TalonFX; import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; public class ShooterReal implements ShooterIO { @@ -54,6 +58,31 @@ public class ShooterReal implements ShooterIO { m_shooter2Motor.setControl(shooter2Velocity.withVelocity(motorRps)); } + @Override + public void setShooterCurrentLimitSpeed( + ShooterState state, + double percentOutput, + Current currentLimit, + AngularVelocity targetVelocity + ) { + state.motor1TargetVelocity = targetVelocity; + state.motor2TargetVelocity = targetVelocity; + + double current = Math.abs(state.motor1Current.in(Amps)) + Math.abs(state.motor2Current.in(Amps)); + double velocity = (Math.abs(state.motor1Velocity.in(RotationsPerSecond)) + Math.abs(state.motor2Velocity.in(RotationsPerSecond))) / 2; + + if( + Math.abs(currentLimit.in(Amps)) > current && + Math.abs(targetVelocity.in(RotationsPerSecond)) > velocity + ) { + m_shooter1Motor.set(percentOutput); + m_shooter2Motor.set(percentOutput); + } else { + m_shooter1Motor.set(0); + m_shooter2Motor.set(0); + } + } + @Override public void setIndexerOutput(ShooterState state, double percentOutput) { state.indexerTargetOutput = percentOutput;