diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 2147a5f..c23c8bb 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -302,9 +302,9 @@ public class ShooterFlywheelConstants { // Velocity PID private static Slot0Configs SLOT0CONFIG = - new Slot0Configs().withKP(1000.0).withKI(0.0).withKD(0.0); + new Slot0Configs().withKP(0.0).withKI(0.0).withKD(0.0).withKS(10.0); - public static TalonFXConfiguration getFXConfig() { + public static TalonFXConfiguration getFXConfig(boolean invert) { TalonFXConfiguration config = new TalonFXConfiguration(); config.CurrentLimits.SupplyCurrentLimitEnable = Robot.isReal(); @@ -319,7 +319,12 @@ public static TalonFXConfiguration getFXConfig() { config.Voltage.PeakReverseVoltage = -12.0; config.MotorOutput.NeutralMode = NeutralModeValue.Brake; - config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + // config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + if (invert == true) { + config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + } else { + config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + } config.SoftwareLimitSwitch.ForwardSoftLimitEnable = false; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 43eb209..70a5332 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -104,12 +104,17 @@ public RobotContainer() { new FlywheelMechanismReal( new MotorIOTalonFX( ShooterFlywheelConstants.NAME, - ShooterFlywheelConstants.getFXConfig(), + ShooterFlywheelConstants.getFXConfig(false), Ports.LeftFlywheel)), new FlywheelMechanismReal( new MotorIOTalonFX( ShooterFlywheelConstants.NAME, - ShooterFlywheelConstants.getFXConfig(), + ShooterFlywheelConstants.getFXConfig(true), + Ports.RightFlywheel)), + new FlywheelMechanismReal( + new MotorIOTalonFX( + ShooterFlywheelConstants.NAME, + ShooterFlywheelConstants.getFXConfig(false), Ports.TowerRoller)), new RotaryMechanismReal( new MotorIOTalonFX( @@ -165,7 +170,15 @@ public RobotContainer() { new FlywheelMechanismSim( new MotorIOTalonFXSim( ShooterFlywheelConstants.NAME, - ShooterFlywheelConstants.getFXConfig(), + ShooterFlywheelConstants.getFXConfig(false), + Ports.LeftFlywheel), + ShooterFlywheelConstants.DCMOTOR, + ShooterFlywheelConstants.MOI, + ShooterFlywheelConstants.TOLERANCE), + new FlywheelMechanismSim( + new MotorIOTalonFXSim( + ShooterFlywheelConstants.NAME, + ShooterFlywheelConstants.getFXConfig(true), Ports.LeftFlywheel), ShooterFlywheelConstants.DCMOTOR, ShooterFlywheelConstants.MOI, @@ -173,7 +186,7 @@ public RobotContainer() { new FlywheelMechanismSim( new MotorIOTalonFXSim( ShooterFlywheelConstants.NAME, - ShooterFlywheelConstants.getFXConfig(), + ShooterFlywheelConstants.getFXConfig(false), Ports.TowerRoller), ShooterFlywheelConstants.DCMOTOR, ShooterFlywheelConstants.MOI, @@ -234,6 +247,7 @@ public RobotContainer() { shooter = new Shooter( + new FlywheelMechanism() {}, new FlywheelMechanism() {}, new FlywheelMechanism() {}, new RotaryMechanism(null, null) {}); @@ -285,17 +299,17 @@ private void configureButtonBindings() { () -> -controller.getRightX())); // Lock to 0° when A button is held - controller - .a() - .whileTrue( - DriveCommands.joystickDriveAtAngle( - drive, - () -> -controller.getLeftY(), - () -> -controller.getLeftX(), - () -> new Rotation2d())); + // controller + // .a() + // .whileTrue( + // DriveCommands.joystickDriveAtAngle( + // drive, + // () -> -controller.getLeftY(), + // () -> -controller.getLeftX(), + // () -> new Rotation2d())); - // Switch to X pattern when X button is pressed - controller.x().onTrue(Commands.runOnce(drive::stopWithX, drive)); + // // Switch to X pattern when X button is pressed + // controller.x().onTrue(Commands.runOnce(drive::stopWithX, drive)); // Reset gyro to 0° when B button is pressed controller @@ -326,6 +340,7 @@ private void configureButtonBindings() { controller.y().onTrue(Commands.runOnce(() -> intake.setVelocity(1))); controller.leftBumper().onTrue(intake.intake()); controller.rightBumper().onTrue(intake.stowAndStopRollers()); + controller.a().onTrue(shooter.runFlywheel()); } /** diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index 31ee0ca..2cc0c7a 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -28,7 +28,8 @@ public class Shooter extends SubsystemBase { - private final FlywheelMechanism _flywheel; + private final FlywheelMechanism _rflywheel; + private final FlywheelMechanism _lflywheel; private final FlywheelMechanism _feeder; private final RotaryMechanism _hood; @@ -36,8 +37,13 @@ public class Shooter extends SubsystemBase { private double desiredVelo; private double hoodAngle; - public Shooter(FlywheelMechanism flywheel, FlywheelMechanism feeder, RotaryMechanism hood) { - _flywheel = flywheel; + public Shooter( + FlywheelMechanism lflywheel, + FlywheelMechanism rflywheel, + FlywheelMechanism feeder, + RotaryMechanism hood) { + _lflywheel = lflywheel; + _rflywheel = rflywheel; _feeder = feeder; _hood = hood; } @@ -53,8 +59,9 @@ public void setFlywheelVelocity(double velocity) { // store the desired velocity then send converted velocity to the mechanism this.desiredVelo = velocity; AngularVelocity angVelo = RotationsPerSecond.of(velocity); - - _flywheel.runVelocity(angVelo, ShooterConstants.ACCELERATION, PIDSlot.SLOT_0); + AngularVelocity negangVelo = RotationsPerSecond.of(velocity); + _lflywheel.runVelocity(angVelo, ShooterConstants.ACCELERATION, PIDSlot.SLOT_0); + _rflywheel.runVelocity(negangVelo, ShooterConstants.ACCELERATION, PIDSlot.SLOT_0); } public enum State { @@ -74,7 +81,9 @@ public enum State { // Checks if the flywheel is at speed and returns a boolean public boolean flyAtVelocity() { - return Math.abs(desiredVelo - _flywheel.getVelocity().in(RotationsPerSecond)) + return ((Math.abs(desiredVelo - _lflywheel.getVelocity().in(RotationsPerSecond)) + + Math.abs(desiredVelo - _rflywheel.getVelocity().in(RotationsPerSecond))) + / 2) <= ShooterConstants.FLYWHEEL_VELOCITY_TOLERANCE; } @@ -129,6 +138,10 @@ public Command shoot(double velocity) { Commands.runOnce(() -> setFlywheelVelocity(0))); } + public Command runFlywheel() { + return Commands.runOnce(() -> setFlywheelVelocity(1)); + } + public void simShoot() { if (Robot.robotContainer.intake.simBalls <= 0) return;