diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 5e0a84d..62793ce 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -54,7 +54,7 @@ public void robotInit() { default: Logger.recordMetadata("GitDirty", "Unknown"); break; - } + } // Set up data receivers & replay source switch (Constants.currentMode) { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 539bed7..4ee60d8 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -35,6 +35,8 @@ import frc.robot.subsystems.pivot.PivotIO; import frc.robot.subsystems.pivot.PivotIOReal; import frc.robot.subsystems.pivot.PivotIOSim; +import frc.robot.subsystems.shooter.Shooter; +import frc.robot.subsystems.shooter.ShooterIOReal; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; /** @@ -48,6 +50,7 @@ public class RobotContainer { private final Drive drive; private final Intake intake; private final Pivot pivot; + private final Shooter shooter; // Controller private final CommandXboxController controller = new CommandXboxController(0); @@ -69,6 +72,7 @@ public RobotContainer() { new ModuleIOTalonFX(3)); intake = Intake.initialize(new IntakeIOReal()); pivot = Pivot.initialize(new PivotIOReal()); + shooter = Shooter.initialize(new ShooterIOReal()); break; case SIM: @@ -82,7 +86,7 @@ public RobotContainer() { new ModuleIOSim()); intake = Intake.initialize(new IntakeIOSim()); pivot = Pivot.initialize(new PivotIOSim()); - + shooter = Shooter.initialize(new ShooterIOReal()); break; default: @@ -96,6 +100,7 @@ public RobotContainer() { new ModuleIO() {}); intake = Intake.initialize(new IntakeIO() {}); pivot = Pivot.initialize(new PivotIO() {}); + shooter = Shooter.initialize(new ShooterIOReal()); // change? break; } @@ -132,23 +137,33 @@ private void configureButtonBindings() { () -> -controller.getLeftX(), () -> -controller.getRightX())); - controller.rightBumper() - .onTrue(Commands.run(() -> intake.setIntakeSpeed(0.75))) - .onFalse(Commands.run(() -> intake.setIntakeSpeed(0))); - - controller.leftBumper() - .onTrue(Commands.run(() -> intake.setIntakeSpeed(-0.75))) - .onFalse(Commands.run(() -> intake.setIntakeSpeed(0))); - - // controller - // .leftBumper() - // .whileTrue(Commands.startEnd(() -> pivot.runVoltage(3), () -> pivot.runVoltage(0), - // pivot)); - - // controller - // .rightBumper() - // .whileTrue(Commands.startEnd(() -> pivot.runVoltage(-3), () -> pivot.runVoltage(0), - // pivot)); + controller + .rightTrigger() + .onTrue(Commands.run(() -> intake.setIntakeSpeed(0.75))) + .onFalse(Commands.run(() -> intake.setIntakeSpeed(0))); + + controller + .leftTrigger() + .onTrue(Commands.run(() -> intake.setIntakeSpeed(-0.75))) + .onFalse(Commands.run(() -> intake.setIntakeSpeed(0))); + + controller + .leftBumper() + .whileTrue(Commands.startEnd(() -> pivot.runVoltage(3), () -> pivot.runVoltage(0), pivot)); + controller + .povUp() + .whileTrue( + Commands.startEnd(() -> shooter.setVoltage(6), () -> shooter.setVoltage(0), pivot)); + // controller.leftBumper() + // .onTrue(Commands.runOnce(() -> pivot.runVoltage(3))) + // .onFalse(Commands.runOnce(() ->pivot.runVoltage(0), pivot)); + // controller.rightBumper() + // .onTrue(Commands.runOnce(() -> pivot.runVoltage(3))) + // .onFalse(Commands.runOnce(() ->pivot.runVoltage(0), pivot)); + + controller + .rightBumper() + .whileTrue(Commands.startEnd(() -> pivot.runVoltage(-3), () -> pivot.runVoltage(0), pivot)); controller.a().whileTrue(Commands.runOnce(() -> drive.zeroHeading(), drive)); } diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index b05ab38..ef904d0 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -24,6 +24,10 @@ private Shooter(ShooterIO shooterIO) { io.updateInputs(inputs); } + public void setVoltage(double voltage) { + io.setVoltage(voltage); + } + @Override public void periodic() { io.updateInputs(inputs); diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java index fe15e61..d6dc2cc 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java @@ -14,5 +14,7 @@ public default void spinForwards() {} public default void spinBackwards() {} + public default void setVoltage(double voltage) {} + public default void updateInputs(ShooterIOInputs inputs) {} } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java index 2c110f0..88b5e8c 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java @@ -6,14 +6,19 @@ public class ShooterIOReal implements ShooterIO { private CANSparkFlex leftShooterMotor = - new CANSparkFlex(0, MotorType.kBrushless); // Can IDs not accurate + new CANSparkFlex(3, MotorType.kBrushless); // Can IDs not accurate private CANSparkFlex rightShooterMotor = - new CANSparkFlex(1, MotorType.kBrushless); // Can IDs not accurate + new CANSparkFlex(12, MotorType.kBrushless); // Can IDs not accurate private RelativeEncoder leftShooterEncoder = leftShooterMotor.getEncoder(); private RelativeEncoder rightShooterEncoder = rightShooterMotor.getEncoder(); public ShooterIOReal() { - rightShooterMotor.follow(leftShooterMotor, true); + rightShooterMotor.restoreFactoryDefaults(); + leftShooterMotor.restoreFactoryDefaults(); + leftShooterMotor.follow(rightShooterMotor, true); + + rightShooterMotor.setSmartCurrentLimit(40); + leftShooterMotor.setSmartCurrentLimit(40); rightShooterMotor.burnFlash(); leftShooterMotor.burnFlash(); @@ -21,12 +26,17 @@ public ShooterIOReal() { @Override public void spinForwards() { - leftShooterMotor.set(1.0); + rightShooterMotor.set(1.0); + } + + @Override + public void setVoltage(double voltage) { + rightShooterMotor.setVoltage(voltage); } @Override public void spinBackwards() { - leftShooterMotor.set(-1.0); + rightShooterMotor.set(-1.0); } public double getLeftvelocity() {