pivot testing from period 9 finished pivot

src/main/java/com/stuypulse/robot/RobotContainer.java

@@ -15,9 +15,11 @@
 import com.stuypulse.robot.commands.pivot.PivotLower;
 import com.stuypulse.robot.commands.pivot.PivotRaise;
 import com.stuypulse.robot.commands.pivot.PivotResetAngle;
+import com.stuypulse.robot.commands.pivot.PivotStop;
 import com.stuypulse.robot.commands.pivot.PivotToAlgaeIntake;
 import com.stuypulse.robot.commands.pivot.PivotToAlgaeStow;
 import com.stuypulse.robot.commands.pivot.PivotToCoralStow;
+import com.stuypulse.robot.commands.pivot.PivotToDirection;
 import com.stuypulse.robot.commands.pivot.SetPivotControlMode;
 import com.stuypulse.robot.commands.pivot.roller.PivotAlgaeIntake;
 import com.stuypulse.robot.commands.pivot.roller.PivotAlgaeOuttake;
@@ -84,19 +86,26 @@ private void configureButtonBindings() {
             .whileTrue(new ClimbToClimb());
         driver.getRightButton()
             .whileTrue(new ClimbToStow());
+        driver.getBottomButton()
+            .whileTrue(new PivotAlgaeOuttake())
+            .onFalse(new PivotRollerStop());
+        driver.getRightMenuButton()
+            .onTrue(new PivotResetAngle());
         // driver.getBottomButton()
         //     .whileTrue(new VisionAlignToReef())
 
         //TRIGGERS
         driver.getRightTriggerButton()
+            .onTrue(new SetPivotControlMode(PivotControlMode.MANUAL))
             .whileTrue(new PivotRaise())
-            .onFalse(new PivotRollerStop());
+            .onFalse(new PivotStop());
         driver.getLeftTriggerButton()
+            .onTrue(new SetPivotControlMode(PivotControlMode.MANUAL))
             .whileTrue(new PivotLower())
-            .onFalse(new PivotRollerStop());
+            .onFalse(new PivotStop());
 
         //BUMPERS
-        driver.getRightBumper() 
+        driver.getRightBumper()
             .whileTrue(new PivotAlgaeOuttake())
             .onFalse(new PivotHoldCoral());
         driver.getLeftBumper()
@@ -106,12 +115,11 @@ private void configureButtonBindings() {
         //DPAD
         driver.getDPadRight()
             .onTrue(new SetPivotControlMode(PivotControlMode.USING_STATES))
-            .onTrue(new PivotToAlgaeStow())
-            .whileFalse(new SetPivotControlMode(PivotControlMode.MANUAL));
+            .onTrue(new PivotToAlgaeStow());
         driver.getDPadDown()
             .onTrue(new SetPivotControlMode(PivotControlMode.USING_STATES))
-            .onTrue(new PivotToAlgaeIntake())
-            .whileFalse(new SetPivotControlMode(PivotControlMode.MANUAL));
+            .onTrue(new PivotToAlgaeIntake());
+
     }
 
     /**************/

src/main/java/com/stuypulse/robot/commands/pivot/PivotResetAngle.java

@@ -1,5 +1,6 @@
 package com.stuypulse.robot.commands.pivot;
 
+import com.stuypulse.robot.constants.Settings;
 import com.stuypulse.robot.subsystems.pivot.Pivot;
 
 import edu.wpi.first.wpilibj2.command.InstantCommand;
@@ -14,7 +15,7 @@ public PivotResetAngle() {
 
     @Override
     public void initialize() {
-        pivot.ResetPivotEncoder();
+        pivot.resetPivotEncoder(Settings.Pivot.DEFAULT_ANGLE.getRotations());
         addRequirements(pivot);
     }
-}
+}

src/main/java/com/stuypulse/robot/commands/pivot/PivotStop.java

@@ -0,0 +1,7 @@
+package com.stuypulse.robot.commands.pivot;
+
+public class PivotStop extends PivotToDirection {
+    public PivotStop () {
+        super(0.0); //Stop Motor Completely
+    }
+}

src/main/java/com/stuypulse/robot/commands/pivot/SetPivotControlMode.java

@@ -3,9 +3,9 @@
 import com.stuypulse.robot.subsystems.pivot.Pivot;
 import com.stuypulse.robot.subsystems.pivot.Pivot.PivotControlMode;
 
-import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
 
-public class SetPivotControlMode extends Command {
+public class SetPivotControlMode extends InstantCommand {
     Pivot pivot = Pivot.getInstance();
     PivotControlMode pivotControlMode;
 
@@ -15,6 +15,6 @@ public SetPivotControlMode (PivotControlMode pivotControlMode) {
 
     @Override
     public void initialize() {
-        pivot.SetPivotControlMode(pivotControlMode);
+        pivot.setPivotControlMode(pivotControlMode);
     }
 }

src/main/java/com/stuypulse/robot/constants/Settings.java

@@ -91,8 +91,8 @@ public interface Pivot {
 
         Rotation2d DEFAULT_ANGLE = Rotation2d.fromDegrees(0);
         Rotation2d CORAL_STOW_ANGLE = Rotation2d.fromDegrees(-3); 
-        Rotation2d ALGAE_HOLDING_ANGLE = Rotation2d.fromDegrees(-15);
-        Rotation2d ALGAE_INTAKE_ANGLE = Rotation2d.fromDegrees(-55);
+        Rotation2d ALGAE_HOLDING_ANGLE = Rotation2d.fromDegrees(-25);
+        Rotation2d ALGAE_INTAKE_ANGLE = Rotation2d.fromDegrees(-70);
         Rotation2d MAX_ANGLE = Rotation2d.fromDegrees(-85);
     }
 

src/main/java/com/stuypulse/robot/subsystems/pivot/Pivot.java

@@ -71,10 +71,11 @@ public String getPivotControlMode() {
 
     public abstract void setPivotMotor(double speed);
 
-    public abstract void ResetPivotEncoder();
+    public abstract void resetPivotEncoder(double newEncoderPosition);
 
-    public abstract void SetPivotControlMode(PivotControlMode SetPivotStateMode);
+    public abstract void setPivotControlMode(PivotControlMode SetPivotStateMode);
 
+    public abstract Rotation2d getPivotRotation();
 
     @Override
     public void periodic() {

src/main/java/com/stuypulse/robot/subsystems/pivot/PivotImpl.java

@@ -24,10 +24,10 @@
 
 public class PivotImpl extends Pivot {
     private SparkMax pivotMotor;
+    private SparkMax rollerMotor;
     private RelativeEncoder pivotEncoder;
 
     private Controller controller;
-    private SparkMax rollerMotor;
 
     private BStream stallDetector;
 
@@ -79,14 +79,14 @@ public void setPivotMotor(double speed) {
         pivotMotor.set(speed);
     }
 
+    @Override
     public Rotation2d getPivotRotation() {
         return Rotation2d.fromRotations(pivotEncoder.getPosition());
     }
 
     @Override
-    public void ResetPivotEncoder() {
-        pivotMotor.getEncoder().setPosition(0);
-        pivotEncoder.setPosition(0); //SmartDashboard.putString("Pivot/Successful reset", );
+    public void resetPivotEncoder(double newEncoderPosition) {
+        pivotEncoder.setPosition(newEncoderPosition);
     }
 
     @Override
@@ -100,7 +100,7 @@ public PivotState getPivotState() {
     }
 
     @Override
-    public void SetPivotControlMode(PivotControlMode pivotControlMode) {
+    public void setPivotControlMode(PivotControlMode pivotControlMode) {
         this.pivotControlMode = pivotControlMode;
     }
 
@@ -109,11 +109,14 @@ public void periodic() {
         super.periodic();
 
         if (stallDetector.getAsBoolean()){
-            // Maybe make this a state? Pivot stalled?
-            setPivotMotor(0);
+            if(rollerMotor.get() > 0) { //Check Stalling Direction: Check if hitting top hard stop
+                resetPivotEncoder(Settings.Pivot.DEFAULT_ANGLE.getRotations());
+            } else if(rollerMotor.get() < 0) { //Check Stalling Direction: Check if hitting bottom hard stop
+                resetPivotEncoder(Settings.Pivot.MAX_ANGLE.getRotations());
+            }
         }
 
-        if (pivotControlMode == PivotControlMode.MANUAL) {
+        if (pivotControlMode == PivotControlMode.USING_STATES) {
             pivotMotor.setVoltage(controller.update(pivotState.targetAngle.getDegrees(), getPivotRotation().getDegrees()));
         }