indexer + handoff methods

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

@@ -28,6 +28,7 @@ import frc.robot.constants.PhotonConstants;
 import frc.robot.constants.ShooterConstants;
 import frc.robot.subsystems.Climber;
 import frc.robot.subsystems.Drivetrain;
+import frc.robot.subsystems.Indexer;
 import frc.robot.utilities.PhotonVision;
 import frc.robot.subsystems.Intake;
 import frc.robot.subsystems.Shooter;
@@ -36,12 +37,13 @@ public class RobotContainer {
     private static final String kAutoTabName = "Autonomous";
     private static final String kTeleopTabName = "Teloperated";
 
-    private PhotonVision photonvision;
+    //private PhotonVision photonvision;
 
     private Drivetrain drivetrain;
     private Intake intake;
     private Shooter shooter;
     private Climber climber;
+    private Indexer indexer;
 
     private CommandXboxController driver;
     private CommandXboxController operator;
@@ -59,7 +61,8 @@ public class RobotContainer {
     private int speakerTag;
 
     public RobotContainer() {
-        try {
+        /*try {
+         
         photonvision = new PhotonVision(
             PhotonConstants.kCameraName, 
             PhotonConstants.kCameraTransform, 
@@ -68,12 +71,12 @@ public class RobotContainer {
         );
         } 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);
+        drivetrain = new Drivetrain(new Pose2d(), null, null);
 
         intake = new Intake();
 
@@ -81,6 +84,8 @@ public class RobotContainer {
 
         climber = new Climber(shooter.getShooterAngle());
 
+        indexer = new Indexer();
+
         // 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());
@@ -133,6 +138,8 @@ public class RobotContainer {
 
         climber.setDefaultCommand(climber.stop());
 
+        indexer.setDefaultCommand(indexer.autoIndexing());
+
         driver.povCenter().onFalse(
         drivetrain.driveCardinal(
             driver::getLeftY, 
@@ -198,7 +205,7 @@ public class RobotContainer {
 
         driver.leftBumper().toggleOnTrue(intake.intakeDownCommand());
 
-        operator.y().onTrue(climber.setSpeed(operator.getRightTriggerAxis()));
+        operator.y().onTrue(climber.setSpeedWithSupplier(operator::getRightTriggerAxis));
         
   }
 

src/main/java/frc/robot/constants/ClimberConstants.java

@@ -1,6 +1,6 @@
 package frc.robot.constants;
 
 public class ClimberConstants {
-    public static final int kClimberMotor1CANID = 1;
+    public static final int kClimberMotor1CANID = 14;
-    public static final int kClimberMotor2CANID = 2;
+    public static final int kClimberMotor2CANID = 15;
 }

src/main/java/frc/robot/constants/IndexerConstants.java

@@ -0,0 +1,7 @@
+package frc.robot.constants;
+
+public class IndexerConstants {
+    public static final int kIndexerID = 13;
+
+    public static final int kIndexerBeamBreakChannel = 3;
+}

src/main/java/frc/robot/constants/ShooterConstants.java

@@ -2,9 +2,9 @@ package frc.robot.constants;
 
 public class ShooterConstants {
     
-    public static final int kTopShooterID = 11;
+    public static final int kTopShooterID = 9;
-    public static final int kBottomShooterID = 12;
+    public static final int kBottomShooterID = 11;
-    public static final int kShooterPivotID = 13;
+    public static final int kShooterPivotID = 16;
 
     public static final double kShooterP = 0.0;
     public static final double kShooterI = 0.0;
@@ -28,4 +28,8 @@ public class ShooterConstants {
 
     public static final double kMaxPivotSpeed = 0.0;
     public static final double kMaxPivotAcceleration = 0.0;
+
+    public static final int kShooterEncoderChannelA = 0;
+    public static final int kShooterEncoderChannelB = 1;
+    public static final int kShooterBeamBreakChannel = 4;
 }

src/main/java/frc/robot/subsystems/Climber.java

@@ -13,17 +13,24 @@ public class Climber extends SubsystemBase {
     private VictorSPX motor1;
     private VictorSPX motor2;
 
+    private DoubleSupplier shooterAngle;
+
     //TODO What tells the climber to stop climbing when it achieves the target height?
     public Climber(DoubleSupplier shooterAngle) {
-        motor1 = new VictorSPX(ClimberConstants.kClimberMotor1CANID);
+        if(shooterAngle.getAsDouble() > Math.toRadians(-10.0)){
-        motor2 = new VictorSPX(ClimberConstants.kClimberMotor2CANID);
+            motor1 = new VictorSPX(ClimberConstants.kClimberMotor1CANID);
+            motor2 = new VictorSPX(ClimberConstants.kClimberMotor2CANID);
 
-        motor2.follow(motor1);
+            motor2.follow(motor1);
+        }
     }
 
     public Command setSpeedWithSupplier(DoubleSupplier speed) {
         return run(() -> {
-            motor1.set(VictorSPXControlMode.PercentOutput, speed.getAsDouble());
+
+            if(shooterAngle.getAsDouble() > Math.toRadians(-10.0)){
+                motor1.set(VictorSPXControlMode.PercentOutput, speed.getAsDouble());
+            }
         });
     }
 

src/main/java/frc/robot/subsystems/Indexer.java

@@ -0,0 +1,43 @@
+package frc.robot.subsystems;
+
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.CANSparkBase.IdleMode;
+import com.revrobotics.CANSparkLowLevel.MotorType;
+
+import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.constants.IndexerConstants;
+
+public class Indexer extends SubsystemBase{
+    
+    private CANSparkMax indexerMotor;
+    private DigitalInput indexerBeamBreak;
+
+    public Indexer(){
+        indexerMotor = new CANSparkMax(IndexerConstants.kIndexerID, MotorType.kBrushed);
+
+        indexerMotor.setSmartCurrentLimit(40);
+        indexerMotor.setIdleMode(IdleMode.kBrake);
+        indexerMotor.burnFlash();
+
+        indexerBeamBreak = new DigitalInput(IndexerConstants.kIndexerBeamBreakChannel);
+    }
+
+    public Command autoIndexing(){
+     return run(() -> {
+        if(!indexerBeamBreak.get()){
+            indexerMotor.set(0.5);
+        }else if(indexerBeamBreak.get()){
+            indexerMotor.set(0.0);
+        }
+
+     });
+    }
+    
+    public Command shootNote(){
+        return run(() -> {
+            indexerMotor.set(1.0);
+        });
+    }
+}

src/main/java/frc/robot/subsystems/Intake.java

@@ -24,7 +24,12 @@ public class Intake extends SubsystemBase{
     public Intake(){
         intakeRoller = new CANSparkMax(IntakeConstants.kIntakeRollerID, MotorType.kBrushless);
         
+        intakeRoller.setSmartCurrentLimit(60);
+        intakeRoller.setIdleMode(IdleMode.kCoast);
+        intakeRoller.burnFlash();
+
         intakePivot = new CANSparkMax(IntakeConstants.kIntakePivotID, MotorType.kBrushless);
+
         intakePivot.setIdleMode(IdleMode.kBrake);
         intakePivot.setSmartCurrentLimit(IntakeConstants.kPivotCurrentLimit);
         intakePivot.burnFlash();

src/main/java/frc/robot/subsystems/Shooter.java

@@ -4,11 +4,13 @@ import java.util.function.DoubleSupplier;
 
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.RelativeEncoder;
+import com.revrobotics.CANSparkBase.IdleMode;
 import com.revrobotics.CANSparkLowLevel.MotorType;
 
 import edu.wpi.first.math.controller.ArmFeedforward;
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj.Encoder;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -34,18 +36,31 @@ public class Shooter extends SubsystemBase{
     private PIDController shooterPivotPID;
     private ArmFeedforward shooterPivotFF;
 
+    private DigitalInput shooterBeamBreak;
+
     public Shooter(){
         topShooter = new CANSparkMax(ShooterConstants.kTopShooterID, MotorType.kBrushless);
         bottomShooter = new CANSparkMax(ShooterConstants.kBottomShooterID, MotorType.kBrushless);
         
+        topShooter.setSmartCurrentLimit(40);
+        topShooter.burnFlash();
+
+        bottomShooter.setSmartCurrentLimit(40);
+        bottomShooter.burnFlash();
+
         topEncoder = topShooter.getEncoder();
         bottomEncoder = bottomShooter.getEncoder();
 
         shooterPivot = new CANSparkMax(ShooterConstants.kShooterPivotID, MotorType.kBrushless);
 
+        shooterPivot.setSmartCurrentLimit(50);
+        shooterPivot.burnFlash();
+
         pivotEncoder = new Encoder(0, 1);
         pivotEncoder.setDistancePerPulse(ShooterConstants.kPivotConversion);
 
+        shooterBeamBreak = new DigitalInput(ShooterConstants.kShooterBeamBreakChannel);
+
         //TODO These can probably devolve into BangBang controllers and let the feed forwards maintain speed
         topShooterPID = new PIDController(
             ShooterConstants.kShooterP, 
@@ -76,6 +91,8 @@ public class Shooter extends SubsystemBase{
 
     public Command angleSpeedsSetpoints(double setpointAngle, double topRPM, double bottomRPM){
         return run(()-> {
+            shooterPivot.setIdleMode(IdleMode.kBrake);
+
             shooterPivot.setVoltage(
                 shooterPivotPID.calculate(pivotEncoder.getDistance(), setpointAngle) + 
                 shooterPivotFF.calculate(setpointAngle, 0.0));
@@ -86,7 +103,32 @@ public class Shooter extends SubsystemBase{
         });
     }
 
+    public Command recieveNote(){
+        return run(() -> {
+            shooterPivot.setIdleMode(IdleMode.kBrake);
+
+            shooterPivot.setVoltage(
+                shooterPivotPID.calculate(pivotEncoder.getDistance(), ShooterConstants.kShooterLoadAngle) + 
+                shooterPivotFF.calculate(ShooterConstants.kShooterLoadAngle, 0.0));
+            
+            if(shooterBeamBreak.get()){
+                topShooter.set(0.25);
+                bottomShooter.set(0.25);
+            }else{
+                topShooter.set(0.);
+                bottomShooter.set(0.); 
+            }
+        });
+    }
+
+    public Command climbState(){
+        return run(() -> {
+            shooterPivot.setIdleMode(IdleMode.kCoast);
+            shooterPivot.set(0.0);
+        });
+    }
+
     public DoubleSupplier getShooterAngle(){
-        return pivotEncoder.getDistance();
+        return () -> {return pivotEncoder.getDistance();};
     }
 }