Adding the start of changes, hood and shooter are separated, indexer modified, still need to do the intake rollers and pivot plus controls and compositions

src/main/java/frc/robot/Commands/AmpHandoff.java

@@ -1,13 +0,0 @@
-package frc.robot.Commands;
-
-import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
-import frc.robot.subsystems.Indexer;
-import frc.robot.subsystems.Shooter;
-
-public class AmpHandoff extends ParallelCommandGroup{
-
-    AmpHandoff(Indexer indexer, Shooter shooter){
-        //addCommands(indexer.shootNote(() -> 1), shooter.ampHandoff());
-    }
-    
-}

src/main/java/frc/robot/Commands/SpeakerShot.java

@@ -1,13 +0,0 @@
-package frc.robot.Commands;
-
-import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
-import frc.robot.constants.ShooterConstants;
-import frc.robot.subsystems.Indexer;
-import frc.robot.subsystems.Shooter;
-
-public class SpeakerShot extends ParallelCommandGroup{
-    
-    SpeakerShot(Indexer indexer, Shooter shooter){
-        //addCommands(shooter.angleSpeedsSetpoints(ShooterConstants.kShooterLoadAngle, 0, 0), indexer.shootNote(() -> 1.0));
-    }
-}

src/main/java/frc/robot/Commands/autoIntaking.java

@@ -1,32 +0,0 @@
-package frc.robot.Commands;
-
-import java.util.function.BooleanSupplier;
-
-import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.constants.IntakeConstants;
-import frc.robot.subsystems.Intake;
-
-public class autoIntaking extends Command{
-    private Intake m_intake;
-    private BooleanSupplier m_beamBreak;
-
-    public autoIntaking(Intake intake, BooleanSupplier beamBreak){
-        m_intake = intake;
-        m_beamBreak = beamBreak;
-
-        addRequirements(intake);
-    }    
-
-    @Override
-    public void execute(){
-        m_intake.intakeControl(() -> IntakeConstants.kDownAngle, () -> 1.0);
-    }
-
-    @Override
-    public boolean isFinished(){
-        if(!m_beamBreak.getAsBoolean()){
-            return true;
-        }else {return false;}
-    }
-
-}

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

@@ -33,7 +33,7 @@ import edu.wpi.first.wpilibj2.command.WaitCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.Commands.autoIntaking;
-import frc.robot.constants.IntakeConstants;
+import frc.robot.constants.IntakeRollerConstants;
 import frc.robot.constants.OIConstants;
 import frc.robot.constants.PhotonConstants;
 import frc.robot.constants.ShooterConstants;
@@ -160,7 +160,7 @@ public class RobotContainer {
         NamedCommands.registerCommand(
             "Auto Intake", 
             intake.intakeControl(
-                () -> IntakeConstants.kDownAngle, 
+                () -> IntakeRollerConstants.kDownAngle, 
                 () -> 1.0
             ).alongWith(
                 indexer.advanceNote()
@@ -237,9 +237,9 @@ public class RobotContainer {
             intake.intakeControl(
                 () -> {
                     if (intakeDown && indexer.getBeamBreak()) {
-                        return IntakeConstants.kDownAngle;
+                        return IntakeRollerConstants.kDownAngle;
                     } else {
-                        return IntakeConstants.kUpAngle;
+                        return IntakeRollerConstants.kUpAngle;
                     }
                 }, 
                 () -> {
@@ -420,7 +420,7 @@ public class RobotContainer {
 
         driver.povDown().whileTrue(indexer.shootNote(() -> -1.0));
 
-        operator.leftTrigger().whileTrue(Commands.parallel( indexer.shootNote(() -> -1), intake.intakeControl(() -> IntakeConstants.kDownAngle, () -> -1.0)));
+        operator.leftTrigger().whileTrue(Commands.parallel( indexer.shootNote(() -> -1), intake.intakeControl(() -> IntakeRollerConstants.kDownAngle, () -> -1.0)));
 
         operator.start().toggleOnTrue(intake.manualPivot(() -> -operator.getRightY()*0.2, () -> driver.getLeftTriggerAxis()));
         

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

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

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

@@ -0,0 +1,42 @@
+package frc.robot.constants;
+
+public class HoodConstants {
+    public enum HoodPositions {
+        kLoadAngle(Math.toRadians(-20.0)),
+        KAmpAngle(Math.toRadians(105.0));
+
+        private double angle;
+
+        private HoodPositions(double angle) {
+            this.angle = angle;
+        }
+
+        public double getAngle() {
+            return angle;
+        }
+    }
+
+
+    public static final int kMotorCANID = 16;
+
+    public static final double kP = 3.0;
+    public static final double kI = 0.0;
+    public static final double kD = 0.0;
+
+    public static final double kLoadAngle = Math.toRadians(-20.0);
+    public static final double KAmpAngle = Math.toRadians(105.0);
+
+    public static final double kEncoderStepConversion = 1/(40.0*(28.0/15.0));
+
+    public static final double kSFF = 0.0;
+    public static final double kGFF = 0.33;
+    public static final double kVFF = 1.44;
+
+    public static final double kMaxVelocity = 0.0;
+    public static final double kMaxAcceleration = 0.0;
+
+    public static final int kMotorCurrentLimit = 50;
+
+    public static final int kEncoderChannelA = 0;
+    public static final int kEncoderChannelB = 1;
+}

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

@@ -1,7 +1,9 @@
 package frc.robot.constants;
 
 public class IndexerConstants {
-    public static final int kIndexerID = 13;
+    public static final int kMotorCANID = 13;
 
-    public static final int kIndexerBeamBreakChannel = 2;
+    public static final int kMotorCurrentLimit = 40;
+
+    public static final int kBeamBreakChannel = 2;
 }

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

@@ -1,8 +1,7 @@
 package frc.robot.constants;
 
-public class IntakeConstants {
+public class IntakePivotConstants {
     public static final int kIntakePivotID = 10;
-    public static final int kIntakeRollerID = 12;
 
     public static final double kIntakePivotConversionFactor = (20.0*(28.0/15.0));
 
@@ -19,5 +18,4 @@ public class IntakeConstants {
     public static final double kStartingAngle = Math.toRadians(95.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/IntakeRollerConstants.java

@@ -0,0 +1,5 @@
+package frc.robot.constants;
+
+public class IntakeRollerConstants {
+    public static final int kMotorCANID = 12;
+}

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

@@ -1,34 +1,10 @@
 package frc.robot.constants;
 
 public class ShooterConstants {
+    public static final int kTopMotorCANID = 9;
+    public static final int kBottomMotorCANID = 11;
 
-    public static final int kTopShooterID = 9;
+    public static final int kMotorCurrentLimit = 40;
-    public static final int kBottomShooterID = 11;
-    public static final int kShooterPivotID = 16;
 
-    public static final double kShooterP = 0.0;
+    public static final int kBeamBreakChannel = 3;
-    public static final double kShooterI = 0.0;
-    public static final double kShooterD = 0.0;
-
-    public static final double kShooterFF = 0.0;
-
-    public static final double kShooterPivotP = 3.0;
-    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;
-    public static final double kGShooterPivotFF = 0.33;
-    public static final double kVShooterPivotFF = 1.44;
-
-    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 = 3;
 }

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

@@ -8,34 +8,23 @@ import com.ctre.phoenix.motorcontrol.can.VictorSPX;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.constants.ClimberConstants;
-import frc.robot.constants.ShooterConstants;
 
 public class Climber extends SubsystemBase {
     private VictorSPX motor1;
     private VictorSPX motor2;
 
-    private DoubleSupplier shooterAngle;
+    public Climber() {
-
+        motor1 = new VictorSPX(ClimberConstants.kMotor1CANID);
-    public Climber(DoubleSupplier shooterAngle) {
+        motor2 = new VictorSPX(ClimberConstants.kMotor2CANID);
-        motor1 = new VictorSPX(ClimberConstants.kClimberMotor1CANID);
-        motor2 = new VictorSPX(ClimberConstants.kClimberMotor2CANID);
-        
         
         motor2.follow(motor1);
         motor1.setInverted(true);
         motor2.setInverted(true);
-        this.shooterAngle = shooterAngle;
     }
 
     public Command setSpeedWithSupplier(DoubleSupplier speed) {
         return run(() -> {
-            if(shooterAngle.getAsDouble() > ShooterConstants.kShooterLoadAngle){
+            motor1.set(VictorSPXControlMode.PercentOutput, speed.getAsDouble());
-           
-                motor1.set(VictorSPXControlMode.PercentOutput, speed.getAsDouble());
-                 
-            }else{
-                motor1.set(VictorSPXControlMode.PercentOutput, 0.0);
-            }
         });
     }
 

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

@@ -20,10 +20,8 @@ import java.util.function.DoubleSupplier;
 import com.kauailabs.navx.frc.AHRS;
 import com.pathplanner.lib.auto.AutoBuilder;
 
-import edu.wpi.first.wpilibj.ADXRS450_Gyro;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.SPI;
-import edu.wpi.first.wpilibj.interfaces.Gyro;
 import frc.robot.constants.AutoConstants;
 import frc.robot.constants.DrivetrainConstants;
 import frc.robot.utilities.MAXSwerveModule;

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

@@ -0,0 +1,109 @@
+package frc.robot.subsystems;
+
+import com.revrobotics.CANSparkLowLevel.MotorType;
+
+import java.util.function.DoubleSupplier;
+
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.CANSparkBase.IdleMode;
+
+import edu.wpi.first.math.controller.ArmFeedforward;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.constants.HoodConstants;
+import frc.robot.constants.HoodConstants.HoodPositions;
+
+public class Hood extends SubsystemBase {
+    private CANSparkMax shooterPivot;
+
+    private Encoder pivotEncoder;
+
+    private PIDController shooterPivotPID;
+    private ArmFeedforward shooterPivotFF;
+
+    private HoodPositions targetPosition;
+
+    private boolean pidControl;
+
+    public Hood() {
+        shooterPivot = new CANSparkMax(HoodConstants.kMotorCANID, MotorType.kBrushless);
+        shooterPivot.setInverted(true);
+
+        shooterPivot.setSmartCurrentLimit(HoodConstants.kMotorCurrentLimit);
+
+        pivotEncoder = new Encoder(0, 1);
+        pivotEncoder.setDistancePerPulse(HoodConstants.kEncoderStepConversion);
+
+        shooterPivot.burnFlash();
+
+        shooterPivotPID = new PIDController(
+            HoodConstants.kP,
+            HoodConstants.kI,
+            HoodConstants.kD
+        );
+
+        shooterPivotFF = new ArmFeedforward(
+            HoodConstants.kSFF,
+            HoodConstants.kGFF,
+            HoodConstants.kVFF
+        );
+
+        shooterPivot.setIdleMode(IdleMode.kBrake);
+
+        targetPosition = HoodPositions.kLoadAngle;
+
+        pidControl = true;
+    }
+
+    @Override
+    public void periodic() {
+        if(pidControl) {
+            shooterPivot.setVoltage(
+                shooterPivotPID.calculate(
+                    getShooterAngle(),
+                    targetPosition.getAngle()
+                ) + shooterPivotFF.calculate(
+                    targetPosition.getAngle(), 
+                    0.0
+                )
+            );
+        }
+    }
+
+    public Command setHoodAngle(HoodPositions newPosition) {
+        return runOnce(() -> {
+            pidControl = true;
+
+            targetPosition = newPosition;
+        });
+    }
+
+    public Command setSpeed(DoubleSupplier speed) {
+        return runOnce(() -> { pidControl = false; }).andThen(run(() -> {
+            shooterPivot.set(speed.getAsDouble());
+        }));
+    }
+
+    public Command climbState() {
+        return runOnce(() -> {
+            pidControl = false;
+            shooterPivot.setIdleMode(IdleMode.kCoast);
+        }).andThen(run(() -> {
+            shooterPivot.set(0);
+        })).finallyDo(() -> {
+            // finallyDo will reset break mode if we back out of climbing and interrupt
+            // the climbState command with something else
+            shooterPivot.setIdleMode(IdleMode.kBrake);
+        });
+    }
+
+    public Command stop() {
+        return setSpeed(() -> 0);
+    }
+
+    public double getShooterAngle() {
+        return pivotEncoder.getDistance() + HoodPositions.kLoadAngle.getAngle();
+    }
+}

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

@@ -13,48 +13,52 @@ 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(){
+    private boolean autoIndex;
-        indexerMotor = new CANSparkMax(IndexerConstants.kIndexerID, MotorType.kBrushed);
 
-        indexerMotor.setSmartCurrentLimit(40);
+    public Indexer(){
+        indexerMotor = new CANSparkMax(IndexerConstants.kMotorCANID, MotorType.kBrushed);
+
+        indexerMotor.setSmartCurrentLimit(IndexerConstants.kMotorCurrentLimit);
         indexerMotor.setIdleMode(IdleMode.kBrake);
         indexerMotor.burnFlash();
 
-        indexerBeamBreak = new DigitalInput(IndexerConstants.kIndexerBeamBreakChannel);
+        indexerBeamBreak = new DigitalInput(IndexerConstants.kBeamBreakChannel);
+
+        autoIndex = false;
     }
 
-    public Command autoIndexing(){
+    @Override
-     return run(() -> {
+    public void periodic() {
-        if(!indexerBeamBreak.get()){
+        if(autoIndex) {
-            indexerMotor.set(0.75);
+            if(!indexerBeamBreak.get()) {
-        }else if(indexerBeamBreak.get()){
+                indexerMotor.set(.75);
-            indexerMotor.set(0.0);
+            } else {
-        }
+                indexerMotor.set(0);
-
-     });
-    }
-
-    public Command advanceNote(){
-        return run(() -> {
-            if(indexerBeamBreak.get()){
-                indexerMotor.set(1);
-            }else{
-                indexerMotor.set(0.0);
             }
-        });
+        }
-    }
-    
-    public Command shootNote(DoubleSupplier indexerSpeed){
-        return run(() -> {
-            indexerMotor.set(indexerSpeed.getAsDouble());
-        });
     }
 
     public boolean getBeamBreak(){
         return indexerBeamBreak.get();
     }
+
+    public Command autoIndex() {
+        return runOnce(() -> {
+            this.autoIndex = true;
+        });
+    }
+    
+    public Command setSpeed(DoubleSupplier speed){
+        return runOnce(() -> { autoIndex = false; }).andThen(run(() -> {
+            indexerMotor.set(speed.getAsDouble());
+        }));
+    }
+
+    public Command stop() {
+        return setSpeed(() -> 0);
+    }
 }

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

@@ -0,0 +1,7 @@
+package frc.robot.subsystems;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class IntakePivot extends SubsystemBase {
+    
+}

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

@@ -13,7 +13,7 @@ import edu.wpi.first.math.util.Units;
 import edu.wpi.first.util.function.BooleanConsumer;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc.robot.constants.IntakeConstants;
+import frc.robot.constants.IntakeRollerConstants;
 
 public class Intake extends SubsystemBase{
     private PIDController intakeAnglePID;
@@ -30,36 +30,36 @@ public class Intake extends SubsystemBase{
     public Intake(){
         armOffset = 0;
 
-        intakeRoller = new CANSparkMax(IntakeConstants.kIntakeRollerID, MotorType.kBrushless);
+        intakeRoller = new CANSparkMax(IntakeRollerConstants.kIntakeRollerID, MotorType.kBrushless);
         
         intakeRoller.setSmartCurrentLimit(60);
         intakeRoller.setIdleMode(IdleMode.kBrake);
         intakeRoller.burnFlash();
 
-        intakePivot = new CANSparkMax(IntakeConstants.kIntakePivotID, MotorType.kBrushless);
+        intakePivot = new CANSparkMax(IntakeRollerConstants.kIntakePivotID, MotorType.kBrushless);
 
         intakePivot.setIdleMode(IdleMode.kBrake);
-        intakePivot.setSmartCurrentLimit(IntakeConstants.kPivotCurrentLimit);
+        intakePivot.setSmartCurrentLimit(IntakeRollerConstants.kPivotCurrentLimit);
         intakePivot.setInverted(true);
         intakePivot.burnFlash();
 
         intakeFeedForward = new ArmFeedforward(
-            IntakeConstants.kSFeedForward, 
+            IntakeRollerConstants.kSFeedForward, 
-            IntakeConstants.kGFeedForward, 
+            IntakeRollerConstants.kGFeedForward, 
-            IntakeConstants.kVFeedForward
+            IntakeRollerConstants.kVFeedForward
         );
 
         intakeEncoder = intakePivot.getEncoder();
-        intakeEncoder.setPosition(Units.radiansToRotations( IntakeConstants.kStartingAngle));
+        intakeEncoder.setPosition(Units.radiansToRotations( IntakeRollerConstants.kStartingAngle));
        // intakeEncoder.setPositionConversionFactor(IntakeConstants.kIntakePivotConversionFactor);
 
         intakeAnglePID = new PIDController(
-            IntakeConstants.kPIntake, 
+            IntakeRollerConstants.kPIntake, 
-            IntakeConstants.kIIntake, 
+            IntakeRollerConstants.kIIntake, 
-            IntakeConstants.kDIntake
+            IntakeRollerConstants.kDIntake
         );
 
-        armOffset = getIntakeAngle()-IntakeConstants.kStartingAngle;
+        armOffset = getIntakeAngle()-IntakeRollerConstants.kStartingAngle;
     }
 
     
@@ -108,9 +108,9 @@ public class Intake extends SubsystemBase{
             intakePivot.setVoltage(
                 intakeAnglePID.calculate(
                     getIntakeAngle(), 
-                    IntakeConstants.kDownAngle
+                    IntakeRollerConstants.kDownAngle
                 ) + intakeFeedForward.calculate(
-                    IntakeConstants.kDownAngle, 
+                    IntakeRollerConstants.kDownAngle, 
                     0.0
                 )
             );
@@ -132,9 +132,9 @@ public class Intake extends SubsystemBase{
             intakePivot.setVoltage(
                 intakeAnglePID.calculate(
                     getIntakeAngle(), 
-                    IntakeConstants.kDownAngle
+                    IntakeRollerConstants.kDownAngle
                 ) + intakeFeedForward.calculate(
-                    IntakeConstants.kDownAngle, 
+                    IntakeRollerConstants.kDownAngle, 
                     0.0
                 )
             );
@@ -148,9 +148,9 @@ public class Intake extends SubsystemBase{
             intakePivot.setVoltage(
                 intakeAnglePID.calculate(
                     getIntakeAngle(), 
-                    IntakeConstants.kUpAngle
+                    IntakeRollerConstants.kUpAngle
                 ) + intakeFeedForward.calculate(
-                    IntakeConstants.kUpAngle, 
+                    IntakeRollerConstants.kUpAngle, 
                     0.0
                 )
             );
@@ -165,15 +165,15 @@ public class Intake extends SubsystemBase{
     }
 
     public double getIntakeAngle(){
-        return Units.rotationsToRadians(intakeEncoder.getPosition()/IntakeConstants.kIntakePivotConversionFactor)-armOffset;
+        return Units.rotationsToRadians(intakeEncoder.getPosition()/IntakeRollerConstants.kIntakePivotConversionFactor)-armOffset;
     }
 
     public double getIntakePID(){
         return intakeAnglePID.calculate(
                     getIntakeAngle(), 
-                    IntakeConstants.kDownAngle
+                    IntakeRollerConstants.kDownAngle
                 ) + intakeFeedForward.calculate(
-                    IntakeConstants.kDownAngle, 
+                    IntakeRollerConstants.kDownAngle, 
                     0.0
                 );
     }

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

@@ -1,163 +1,53 @@
 package frc.robot.subsystems;
 
-import java.util.function.BooleanSupplier;
 import java.util.function.DoubleSupplier;
 
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.CANSparkBase.IdleMode;
 import com.revrobotics.CANSparkLowLevel.MotorType;
 
-import edu.wpi.first.math.MathUtil;
-import edu.wpi.first.math.controller.ArmFeedforward;
-import edu.wpi.first.math.controller.PIDController;
 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;
 import frc.robot.constants.ShooterConstants;
 
-public class Shooter extends SubsystemBase{
+public class Shooter extends SubsystemBase {
     private CANSparkMax topShooter;
     private CANSparkMax bottomShooter;
 
-    private CANSparkMax shooterPivot;
-
-    private Encoder pivotEncoder;
-
-    private PIDController shooterPivotPID;
-    private ArmFeedforward shooterPivotFF;
-
     private DigitalInput shooterBeamBreak;
 
-    private boolean indexerBeamBreak;
+    public Shooter(){
-
+        topShooter = new CANSparkMax(ShooterConstants.kTopMotorCANID, MotorType.kBrushless);
-    public Shooter(BooleanSupplier indexerBeamBreak){
-        topShooter = new CANSparkMax(ShooterConstants.kTopShooterID, MotorType.kBrushless);
         topShooter.setInverted(true);
-        bottomShooter = new CANSparkMax(ShooterConstants.kBottomShooterID, MotorType.kBrushless);
+
+        bottomShooter = new CANSparkMax(ShooterConstants.kBottomMotorCANID, MotorType.kBrushless);
         bottomShooter.setInverted(true);
         
-        shooterPivot = new CANSparkMax(ShooterConstants.kShooterPivotID, MotorType.kBrushless);
+        topShooter.setSmartCurrentLimit(ShooterConstants.kMotorCurrentLimit);
-        shooterPivot.setInverted(true);
+        bottomShooter.setSmartCurrentLimit(ShooterConstants.kMotorCurrentLimit);
 
-        /* 
+        shooterBeamBreak = new DigitalInput(ShooterConstants.kBeamBreakChannel);
-        topPID = topShooter.getPIDController();
-        topPID.setFeedbackDevice(topEncoder);
-        bottomPID = bottomShooter.getPIDController();
-        bottomPID.setFeedbackDevice(bottomEncoder);
-        */
-
-        shooterPivot.setSmartCurrentLimit(50);
-        topShooter.setSmartCurrentLimit(40);
-        bottomShooter.setSmartCurrentLimit(40);
-
-        pivotEncoder = new Encoder(0, 1);
-        pivotEncoder.setDistancePerPulse(ShooterConstants.kPivotConversion);
-
-        shooterBeamBreak = new DigitalInput(ShooterConstants.kShooterBeamBreakChannel);
 
         topShooter.setIdleMode(IdleMode.kCoast);
         bottomShooter.setIdleMode(IdleMode.kCoast);
 
-        bottomShooter.burnFlash();
-        shooterPivot.burnFlash();
         topShooter.burnFlash();
-
+        bottomShooter.burnFlash();
-        shooterPivotPID = new PIDController(
-            ShooterConstants.kShooterPivotP,
-            ShooterConstants.kShooterPivotI,
-            ShooterConstants.kShooterPivotD
-        );
-        shooterPivotFF = new ArmFeedforward(ShooterConstants.kSShooterPivotFF, ShooterConstants.kGShooterPivotFF, ShooterConstants.kVShooterPivotFF);
-
     }
  
-    /*
+    public boolean getBeamBreak(){
-    public Command angleSpeedsSetpoints(double setpointAngle, double topRPM, double bottomRPM){
-        return run(()-> {
-            angleAndSpeedControl(setpointAngle, topRPM, bottomRPM);
-        });
-    }*/
-
-    public Command angleSpeedsSetpoints(DoubleSupplier setpointAngle, DoubleSupplier speed){
-        return run(()-> {
-            angleAndSpeedControl(setpointAngle.getAsDouble(), speed.getAsDouble(), speed.getAsDouble());
-        });
-    }
-
-    public Command angleSpeedsSetpoints(DoubleSupplier setpointAngle, double topRPM, double bottomRPM){
-        return run(()-> {
-            angleAndSpeedControl(setpointAngle.getAsDouble(), topRPM, bottomRPM);
-        });
-    }
-
-    private void angleAndSpeedControl(double setpointAngle, double topRPM, double bottomRPM){
-        shooterPivot.setIdleMode(IdleMode.kBrake);
-
-            shooterPivot.setVoltage(
-                shooterPivotPID.calculate(getShooterAngle(), setpointAngle) + 
-                shooterPivotFF.calculate(setpointAngle, 0.0));
-
-            //topPID.setReference(topRPM, CANSparkMax.ControlType.kVelocity);
-            //bottomPID.setReference(bottomRPM, CANSparkMax.ControlType.kVelocity);
-            topShooter.set(topRPM);
-            bottomShooter.set(bottomRPM);
-    }
-
-    public Command ampHandoff(DoubleSupplier shootNoteSpeed, BooleanSupplier ampAngleOrNah){
-        return run(() -> {
-            
-            shooterPivot.setIdleMode(IdleMode.kBrake);
-
-            shooterPivot.setVoltage(
-                shooterPivotPID.calculate(getShooterAngle(), ShooterConstants.kShooterLoadAngle) + 
-                shooterPivotFF.calculate(ShooterConstants.kShooterLoadAngle, 0.0));
-            
-            if(shooterBeamBreak.get() || indexerBeamBreak){
-                angleAndSpeedControl(ShooterConstants.kShooterLoadAngle, 0.25, 0.25);
-            }else{
-                angleAndSpeedControl(ShooterConstants.kShooterLoadAngle, shootNoteSpeed.getAsDouble(), shootNoteSpeed.getAsDouble());
-            }
-        });
-    }
-
-    public Command climbState(){
-        return run(() -> {
-            shooterPivot.setIdleMode(IdleMode.kCoast);
-            shooterPivot.set(0.0);
-        });
-    }
-
-    public double getShooterAngle(){
-        return pivotEncoder.getDistance() + ShooterConstants.kShooterLoadAngle;
-    }
-
-    public Command zeroEncoder(){
-        return run(() -> {
-            pivotEncoder.reset();
-        });
-    }
-
-    public Boolean getBeamBreak(){
         return shooterBeamBreak.get();
     }
 
-    public Command manualPivot(DoubleSupplier pivotPower, DoubleSupplier spinny){
+    public Command setSpeed(DoubleSupplier speed) {
-        return run(() ->{
+        return run(() -> {
-            shooterPivot.setIdleMode(IdleMode.kBrake);
+            topShooter.set(speed.getAsDouble());
-            if(getShooterAngle() >= ShooterConstants.kShooterLoadAngle && getShooterAngle() <= ShooterConstants.kShooterAmpAngle){
+            bottomShooter.set(speed.getAsDouble());
-                shooterPivot.set(pivotPower.getAsDouble());
-            }else if(getShooterAngle() > ShooterConstants.kShooterAmpAngle){
-                shooterPivot.set(MathUtil.clamp(pivotPower.getAsDouble(), -1.0, 0.0));
-            }else if(getShooterAngle() < ShooterConstants.kShooterLoadAngle){
-                shooterPivot.set(MathUtil.clamp(pivotPower.getAsDouble(), 0.0, 1.0));
-            }
-
-            topShooter.set(spinny.getAsDouble());
-            bottomShooter.set(spinny.getAsDouble());
-            
         });
     }
 
-
+    public Command stop() {
+        return setSpeed(() -> 0);
+    }
 }