shooter and intake command

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 kAutoTabName = "Autonomous";
-  private static final String kTeleopTabName = "Teloperated";
+    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 driver;
-  private CommandXboxController operator;
+    private CommandXboxController operator;
 
-  private Trigger isTeleopTrigger;
+    private Trigger isTeleopTrigger;
 
-  private SendableChooser<Command> autoChooser;
+    private SendableChooser<Command> autoChooser;
 
-  private int ampTagID;
+    private int ampTagID;
 
-  // TODO There's more than one source tag, how do we want to handle this?
+    // TODO There's more than one source tag, how do we want to handle this?
-  private int sourceTagID;
+    private int sourceTagID;
 
-  // TODO There's more than one speaker tag, how do we want to handle this?
+    // TODO There's more than one speaker tag, how do we want to handle this?
-  private int speakerTag;
+    private int speakerTag;
 
-  public RobotContainer() {
+    public RobotContainer() {
-    try {
+        try {
-      photonvision = new PhotonVision(
+        photonvision = new PhotonVision(
-        PhotonConstants.kCameraName, 
+            PhotonConstants.kCameraName, 
-        PhotonConstants.kCameraTransform, 
+            PhotonConstants.kCameraTransform, 
-        PhotonConstants.kCameraHeightMeters,
+            PhotonConstants.kCameraHeightMeters,
-        PhotonConstants.kCameraPitchRadians
+            PhotonConstants.kCameraPitchRadians
-      );
+        );
-    } catch (IOException e) {
+        } catch (IOException e) {
-      photonvision = null;
+        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<DriverStation.Alliance> 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
+    // The objective should be to have the subsystems expose methods that return commands
-    // TODO Need to provide a real IAprilTagProvider, null means we're not using one at all
+    // that can be bound to the triggers provided by the CommandXboxController class.
-    // TODO Need to provide a real VisualPoseProvider, null means we're not using one at all
+    // This mindset should help keep RobotContainer a little cleaner this year. 
-    drivetrain = new Drivetrain(new Pose2d(), photonvision, null);
+    // 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
+        drivetrain.setDefaultCommand(
-    // ALL Named Commands need to be defined AFTER subsystem initialization and BEFORE auto/controller configuration
+        drivetrain.teleopCommand(
-    NamedCommands.registerCommand("Set Drivetrain X", drivetrain.setXCommand());
+            driver::getLeftY, 
-
+            driver::getLeftX, 
-    // TODO Specify a default auto string once we have one
+            driver::getRightX, 
-    autoChooser = AutoBuilder.buildAutoChooser();
+            OIConstants.kTeleopDriveDeadband
-
-    driver = new CommandXboxController(OIConstants.kPrimaryXboxUSB);
-    //secondary = new CommandXboxController(OIConstants.kSecondaryXboxUSB);
-
-    isTeleopTrigger = new Trigger(DriverStation::isTeleopEnabled);
-
-    Optional<DriverStation.Alliance> 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
         )
-    );
+        );
 
-    // Lock on to the appropriate Amp AprilTag while still being able to strafe and drive forward and back
+        intake.setDefaultCommand(intake.intakeUpCommand());
-    driver.a().onTrue(
-      drivetrain.driveAprilTagLock(
-        driver::getLeftY, 
-        driver::getLeftX, 
-        OIConstants.kTeleopDriveDeadband, 
-        ampTagID
-      ).until(
-        () -> MathUtil.applyDeadband(driver.getRightX(), OIConstants.kTeleopDriveDeadband) != 0
-      )
-    );
 
-    // Lock on to the appropriate Source AprilTag while still being able to strafe and drive forward and back
+        shooter.setDefaultCommand(shooter.angleSpeedsSetpoints(ShooterConstants.kShooterLoadAngle, 0.0, 0.0));
-    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
+        climber.setDefaultCommand(climber.stop());
-    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.povCenter().onFalse(
-    driver.y().onTrue(drivetrain.zeroHeadingCommand());
+        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
+        // Lock on to the appropriate Amp AprilTag while still being able to strafe and drive forward and back
-    driver.rightBumper().onTrue(drivetrain.setXCommand());
+        driver.a().onTrue(
+        drivetrain.driveAprilTagLock(
+            driver::getLeftY, 
+            driver::getLeftX, 
+            OIConstants.kTeleopDriveDeadband, 
+            ampTagID
+        ).until(
+            () -> MathUtil.applyDeadband(driver.getRightX(), OIConstants.kTeleopDriveDeadband) != 0
+        )
+        );
 
-    /*
+        // Lock on to the appropriate Source AprilTag while still being able to strafe and drive forward and back
-     * This has been added because interest has been expressed in trying field relative vs
+        driver.b().onTrue(
-     * robot relative control. This should <i>default</i> to field relative, but give the option
+        drivetrain.driveAprilTagLock(
-     * for the person practicing driving to push start on the driver's controller to quickly switch to
+            driver::getLeftY, 
-     * the other style. 
+            driver::getLeftX,  
-     * 
+            OIConstants.kTeleopDriveDeadband, 
-     * If it becomes something that we need to switch <i>prior</i> to the start of the match, a different
+            sourceTagID
-     * mechanism will need to be devised, this will work for now. 
+        ).until(
-     */
+            () -> MathUtil.applyDeadband(driver.getRightX(), OIConstants.kTeleopDriveDeadband) != 0
-    driver.start().onTrue(drivetrain.toggleFieldRelativeControl());
+        )
+        );
+
+        // 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 <i>default</i> 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 <i>prior</i> 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)
+        .withPosition(0, 0)
-      .withSize(2, 1)
+        .withSize(2, 1)
-      .withWidget(BuiltInWidgets.kBooleanBox);
+        .withWidget(BuiltInWidgets.kBooleanBox);
   }
 
   public Command getAutonomousCommand() {
-    return autoChooser.getSelected();
+        return autoChooser.getSelected();
   }
 }

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(105.0);
-    public static final double kStartingAngle = Math.toRadians(110.0);
+    public static final double kUpAngle = Math.toRadians(90.0);
-    public static final double kUpAngle = Math.toRadians(100.0);
+    public static final double kDownAngle = Math.toRadians(-34.0);
-    public static final double kDownAngle = Math.toRadians(-40.0);
 }

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;

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);
 

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();
+    }
 }