diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d068af6..f634214 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -25,157 +25,181 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.constants.OIConstants; import frc.robot.constants.PhotonConstants; +import frc.robot.constants.ShooterConstants; +import frc.robot.subsystems.Climber; import frc.robot.subsystems.Drivetrain; import frc.robot.utilities.PhotonVision; +import frc.robot.subsystems.Intake; +import frc.robot.subsystems.Shooter; public class RobotContainer { - private static final String kAutoTabName = "Autonomous"; - private static final String kTeleopTabName = "Teloperated"; + private static final String kAutoTabName = "Autonomous"; + private static final String kTeleopTabName = "Teloperated"; - private PhotonVision photonvision; + private PhotonVision photonvision; - private Drivetrain drivetrain; + private Drivetrain drivetrain; + private Intake intake; + private Shooter shooter; + private Climber climber; - private CommandXboxController driver; - private CommandXboxController operator; + private CommandXboxController driver; + private CommandXboxController operator; - private Trigger isTeleopTrigger; + private Trigger isTeleopTrigger; - private SendableChooser autoChooser; + private SendableChooser autoChooser; - private int ampTagID; + private int ampTagID; - // TODO There's more than one source tag, how do we want to handle this? - private int sourceTagID; + // TODO There's more than one source tag, how do we want to handle this? + private int sourceTagID; - // TODO There's more than one speaker tag, how do we want to handle this? - private int speakerTag; + // TODO There's more than one speaker tag, how do we want to handle this? + private int speakerTag; - public RobotContainer() { - try { - photonvision = new PhotonVision( - PhotonConstants.kCameraName, - PhotonConstants.kCameraTransform, - PhotonConstants.kCameraHeightMeters, - PhotonConstants.kCameraPitchRadians - ); - } catch (IOException e) { - photonvision = null; + public RobotContainer() { + try { + photonvision = new PhotonVision( + PhotonConstants.kCameraName, + PhotonConstants.kCameraTransform, + PhotonConstants.kCameraHeightMeters, + PhotonConstants.kCameraPitchRadians + ); + } catch (IOException e) { + photonvision = null; + } + + // TODO Need to provide a real initial pose + // TODO Need to provide a real IAprilTagProvider, null means we're not using one at all + // TODO Need to provide a real VisualPoseProvider, null means we're not using one at all + drivetrain = new Drivetrain(new Pose2d(), photonvision, null); + + intake = new Intake(); + + shooter = new Shooter(); + + climber = new Climber(shooter.getShooterAngle()); + + // An example Named Command, doesn't need to remain once we start actually adding real things + // ALL Named Commands need to be defined AFTER subsystem initialization and BEFORE auto/controller configuration + NamedCommands.registerCommand("Set Drivetrain X", drivetrain.setXCommand()); + + // TODO Specify a default auto string once we have one + autoChooser = AutoBuilder.buildAutoChooser(); + + driver = new CommandXboxController(OIConstants.kPrimaryXboxUSB); + operator = new CommandXboxController(OIConstants.kSecondaryXboxUSB); + + isTeleopTrigger = new Trigger(DriverStation::isTeleopEnabled); + + Optional alliance = DriverStation.getAlliance(); + + if (alliance.isEmpty() || alliance.get() == DriverStation.Alliance.Red) { + ampTagID = 5; + sourceTagID = 9; + speakerTag = 4; + } else { + ampTagID = 6; + sourceTagID = 1; + speakerTag = 8; + } + + configureBindings(); + shuffleboardSetup(); } - // TODO Need to provide a real initial pose - // TODO Need to provide a real IAprilTagProvider, null means we're not using one at all - // TODO Need to provide a real VisualPoseProvider, null means we're not using one at all - drivetrain = new Drivetrain(new Pose2d(), photonvision, null); + // The objective should be to have the subsystems expose methods that return commands + // that can be bound to the triggers provided by the CommandXboxController class. + // This mindset should help keep RobotContainer a little cleaner this year. + // This is not to say you can't trigger or command chain here (see driveCardnial drivetrain example), + // but generally reduce/avoid any situation where the keyword "new" is involved if you're working + // with a subsystem + private void configureBindings() { + isTeleopTrigger.onTrue(new InstantCommand(() -> Shuffleboard.selectTab(kTeleopTabName))); - // An example Named Command, doesn't need to remain once we start actually adding real things - // ALL Named Commands need to be defined AFTER subsystem initialization and BEFORE auto/controller configuration - NamedCommands.registerCommand("Set Drivetrain X", drivetrain.setXCommand()); - - // TODO Specify a default auto string once we have one - autoChooser = AutoBuilder.buildAutoChooser(); - - driver = new CommandXboxController(OIConstants.kPrimaryXboxUSB); - //secondary = new CommandXboxController(OIConstants.kSecondaryXboxUSB); - - isTeleopTrigger = new Trigger(DriverStation::isTeleopEnabled); - - Optional alliance = DriverStation.getAlliance(); - - if (alliance.isEmpty() || alliance.get() == DriverStation.Alliance.Red) { - ampTagID = 5; - sourceTagID = 9; - speakerTag = 4; - } else { - ampTagID = 6; - sourceTagID = 1; - speakerTag = 8; - } - - configureBindings(); - shuffleboardSetup(); - } - - // The objective should be to have the subsystems expose methods that return commands - // that can be bound to the triggers provided by the CommandXboxController class. - // This mindset should help keep RobotContainer a little cleaner this year. - // This is not to say you can't trigger or command chain here (see driveCardnial drivetrain example), - // but generally reduce/avoid any situation where the keyword "new" is involved if you're working - // with a subsystem - private void configureBindings() { - isTeleopTrigger.onTrue(new InstantCommand(() -> Shuffleboard.selectTab(kTeleopTabName))); - - drivetrain.setDefaultCommand( - drivetrain.teleopCommand( - driver::getLeftY, - driver::getLeftX, - driver::getRightX, - OIConstants.kTeleopDriveDeadband - ) - ); - - driver.povCenter().onFalse( - drivetrain.driveCardinal( - driver::getLeftY, - driver::getLeftX, - driver.getHID()::getPOV, - OIConstants.kTeleopDriveDeadband).until( - () -> MathUtil.applyDeadband(driver.getRightX(), OIConstants.kTeleopDriveDeadband) != 0 + drivetrain.setDefaultCommand( + drivetrain.teleopCommand( + driver::getLeftY, + driver::getLeftX, + driver::getRightX, + OIConstants.kTeleopDriveDeadband ) - ); + ); - // Lock on to the appropriate Amp AprilTag while still being able to strafe and drive forward and back - driver.a().onTrue( - drivetrain.driveAprilTagLock( - driver::getLeftY, - driver::getLeftX, - OIConstants.kTeleopDriveDeadband, - ampTagID - ).until( - () -> MathUtil.applyDeadband(driver.getRightX(), OIConstants.kTeleopDriveDeadband) != 0 - ) - ); + intake.setDefaultCommand(intake.intakeUpCommand()); - // Lock on to the appropriate Source AprilTag while still being able to strafe and drive forward and back - driver.b().onTrue( - drivetrain.driveAprilTagLock( - driver::getLeftY, - driver::getLeftX, - OIConstants.kTeleopDriveDeadband, - sourceTagID - ).until( - () -> MathUtil.applyDeadband(driver.getRightX(), OIConstants.kTeleopDriveDeadband) != 0 - ) - ); + shooter.setDefaultCommand(shooter.angleSpeedsSetpoints(ShooterConstants.kShooterLoadAngle, 0.0, 0.0)); - // Lock on to the appropriate Speaker AprilTag while still being able to strafe and drive forward and back - driver.x().onTrue( - drivetrain.driveAprilTagLock( - driver::getLeftY, - driver::getLeftX, - OIConstants.kTeleopDriveDeadband, - speakerTag - ).until( - () -> MathUtil.applyDeadband(driver.getRightX(), OIConstants.kTeleopDriveDeadband) != 0 - ) - ); + climber.setDefaultCommand(climber.stop()); - // This was originally a run while held, not sure that's really necessary, change it if need be - driver.y().onTrue(drivetrain.zeroHeadingCommand()); + driver.povCenter().onFalse( + drivetrain.driveCardinal( + driver::getLeftY, + driver::getLeftX, + driver.getHID()::getPOV, + OIConstants.kTeleopDriveDeadband).until( + () -> MathUtil.applyDeadband(driver.getRightX(), OIConstants.kTeleopDriveDeadband) != 0 + ) + ); - // This was originally a run while held, not sure that's really necessary, change it if need be - driver.rightBumper().onTrue(drivetrain.setXCommand()); + // Lock on to the appropriate Amp AprilTag while still being able to strafe and drive forward and back + driver.a().onTrue( + drivetrain.driveAprilTagLock( + driver::getLeftY, + driver::getLeftX, + OIConstants.kTeleopDriveDeadband, + ampTagID + ).until( + () -> MathUtil.applyDeadband(driver.getRightX(), OIConstants.kTeleopDriveDeadband) != 0 + ) + ); - /* - * This has been added because interest has been expressed in trying field relative vs - * robot relative control. This should default to field relative, but give the option - * for the person practicing driving to push start on the driver's controller to quickly switch to - * the other style. - * - * If it becomes something that we need to switch prior to the start of the match, a different - * mechanism will need to be devised, this will work for now. - */ - driver.start().onTrue(drivetrain.toggleFieldRelativeControl()); + // Lock on to the appropriate Source AprilTag while still being able to strafe and drive forward and back + driver.b().onTrue( + drivetrain.driveAprilTagLock( + driver::getLeftY, + driver::getLeftX, + OIConstants.kTeleopDriveDeadband, + sourceTagID + ).until( + () -> MathUtil.applyDeadband(driver.getRightX(), OIConstants.kTeleopDriveDeadband) != 0 + ) + ); + + // Lock on to the appropriate Speaker AprilTag while still being able to strafe and drive forward and back + driver.x().onTrue( + drivetrain.driveAprilTagLock( + driver::getLeftY, + driver::getLeftX, + OIConstants.kTeleopDriveDeadband, + speakerTag + ).until( + () -> MathUtil.applyDeadband(driver.getRightX(), OIConstants.kTeleopDriveDeadband) != 0 + ) + ); + + // This was originally a run while held, not sure that's really necessary, change it if need be + driver.y().onTrue(drivetrain.zeroHeadingCommand()); + + // This was originally a run while held, not sure that's really necessary, change it if need be + driver.rightBumper().onTrue(drivetrain.setXCommand()); + + /* + * This has been added because interest has been expressed in trying field relative vs + * robot relative control. This should default to field relative, but give the option + * for the person practicing driving to push start on the driver's controller to quickly switch to + * the other style. + * + * If it becomes something that we need to switch prior to the start of the match, a different + * mechanism will need to be devised, this will work for now. + */ + driver.start().onTrue(drivetrain.toggleFieldRelativeControl()); + + driver.leftBumper().toggleOnTrue(intake.intakeDownCommand()); + + operator.y().onTrue(climber.setSpeed(operator.getRightTriggerAxis())); + } private void shuffleboardSetup() { @@ -190,12 +214,12 @@ public class RobotContainer { ShuffleboardTab teleopTab = Shuffleboard.getTab(kTeleopTabName); teleopTab.addBoolean("Field Relative Controls Enabled?", drivetrain::isFieldRelativeControl) - .withPosition(0, 0) - .withSize(2, 1) - .withWidget(BuiltInWidgets.kBooleanBox); + .withPosition(0, 0) + .withSize(2, 1) + .withWidget(BuiltInWidgets.kBooleanBox); } public Command getAutonomousCommand() { - return autoChooser.getSelected(); + return autoChooser.getSelected(); } } diff --git a/src/main/java/frc/robot/constants/IntakeConstants.java b/src/main/java/frc/robot/constants/IntakeConstants.java index 8497da5..d1b3a4e 100644 --- a/src/main/java/frc/robot/constants/IntakeConstants.java +++ b/src/main/java/frc/robot/constants/IntakeConstants.java @@ -16,8 +16,7 @@ public class IntakeConstants { public static final double kGFeedForward = 0; public static final double kVFeedForward = 0; - - public static final double kStartingAngle = Math.toRadians(110.0); - public static final double kUpAngle = Math.toRadians(100.0); - public static final double kDownAngle = Math.toRadians(-40.0); + public static final double kStartingAngle = Math.toRadians(105.0); + public static final double kUpAngle = Math.toRadians(90.0); + public static final double kDownAngle = Math.toRadians(-34.0); } diff --git a/src/main/java/frc/robot/constants/ShooterConstants.java b/src/main/java/frc/robot/constants/ShooterConstants.java index d43f15c..075ee97 100644 --- a/src/main/java/frc/robot/constants/ShooterConstants.java +++ b/src/main/java/frc/robot/constants/ShooterConstants.java @@ -17,6 +17,9 @@ public class ShooterConstants { public static final double kShooterPivotI = 0.0; public static final double kShooterPivotD = 0.0; + public static final double kShooterLoadAngle = Math.toRadians(-20.0); + public static final double kShooterAmpAngle = Math.toRadians(105.0); + public static final double kPivotConversion = 1/(40.0*(28.0/15.0)); public static final double kSShooterPivotFF = 0.0; diff --git a/src/main/java/frc/robot/subsystems/Climber.java b/src/main/java/frc/robot/subsystems/Climber.java index 8e2e4d3..ecc66ce 100644 --- a/src/main/java/frc/robot/subsystems/Climber.java +++ b/src/main/java/frc/robot/subsystems/Climber.java @@ -14,7 +14,7 @@ public class Climber extends SubsystemBase { private VictorSPX motor2; //TODO What tells the climber to stop climbing when it achieves the target height? - public Climber() { + public Climber(DoubleSupplier shooterAngle) { motor1 = new VictorSPX(ClimberConstants.kClimberMotor1CANID); motor2 = new VictorSPX(ClimberConstants.kClimberMotor2CANID); diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index ebf7ae9..5c8861b 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -1,5 +1,7 @@ package frc.robot.subsystems; +import java.util.function.DoubleSupplier; + import com.revrobotics.CANSparkMax; import com.revrobotics.RelativeEncoder; import com.revrobotics.CANSparkLowLevel.MotorType; @@ -79,8 +81,12 @@ public class Shooter extends SubsystemBase{ shooterPivotFF.calculate(setpointAngle, 0.0)); topShooter.setVoltage(topShooterPID.calculate(topEncoder.getVelocity(), topRPM)+topShooterFF.calculate(topRPM)); - + bottomShooter.setVoltage(bottomShooterPID.calculate(bottomEncoder.getVelocity(), bottomRPM)+bottomShooterFF.calculate(bottomRPM)); }); } + + public DoubleSupplier getShooterAngle(){ + return pivotEncoder.getDistance(); + } }