shooter and intake command

src/main/java/frc/robot/RobotContainer.java

@@ -25,8 +25,12 @@ 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";
@@ -35,6 +39,9 @@ public class RobotContainer {
     private PhotonVision photonvision;
 
     private Drivetrain drivetrain;
+    private Intake intake;
+    private Shooter shooter;
+    private Climber climber;
 
     private CommandXboxController driver;
     private CommandXboxController operator;
@@ -68,6 +75,12 @@ public class RobotContainer {
         // 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());
@@ -76,7 +89,7 @@ public class RobotContainer {
         autoChooser = AutoBuilder.buildAutoChooser();
 
         driver = new CommandXboxController(OIConstants.kPrimaryXboxUSB);
-    //secondary = new CommandXboxController(OIConstants.kSecondaryXboxUSB);
+        operator = new CommandXboxController(OIConstants.kSecondaryXboxUSB);
 
         isTeleopTrigger = new Trigger(DriverStation::isTeleopEnabled);
 
@@ -114,6 +127,12 @@ public class RobotContainer {
         )
         );
 
+        intake.setDefaultCommand(intake.intakeUpCommand());
+
+        shooter.setDefaultCommand(shooter.angleSpeedsSetpoints(ShooterConstants.kShooterLoadAngle, 0.0, 0.0));
+
+        climber.setDefaultCommand(climber.stop());
+
         driver.povCenter().onFalse(
         drivetrain.driveCardinal(
             driver::getLeftY, 
@@ -176,6 +195,11 @@ public class RobotContainer {
         * 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() {

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

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;
@@ -83,4 +85,8 @@ public class Shooter extends SubsystemBase{
             bottomShooter.setVoltage(bottomShooterPID.calculate(bottomEncoder.getVelocity(), bottomRPM)+bottomShooterFF.calculate(bottomRPM));
         });
     }
+
+    public DoubleSupplier getShooterAngle(){
+        return pivotEncoder.getDistance();
+    }
 }