From f35365bb2d299f27520f609faa859aef0cd226f9 Mon Sep 17 00:00:00 2001
From: beansbeansbeansyes <816215@seq.org>
Date: Thu, 8 Sep 2022 19:29:32 -0700
Subject: [PATCH 01/28] make

---
 .../team199/robot2022/subsystems/Climber.java | 45 ++++++++++---------
 1 file changed, 25 insertions(+), 20 deletions(-)

diff --git a/src/main/java/org/team199/robot2022/subsystems/Climber.java b/src/main/java/org/team199/robot2022/subsystems/Climber.java
index 1824a2f..f289c9c 100644
--- a/src/main/java/org/team199/robot2022/subsystems/Climber.java
+++ b/src/main/java/org/team199/robot2022/subsystems/Climber.java
@@ -31,6 +31,18 @@ public class Climber extends SubsystemBase {
 
     private static final double retractPositionLeft = -1.151;
     private static final double retractPositionRight = -1.172;
+    public enum encoderResetPos {
+        public enum left {
+            public static double extended=5.317;
+            public static double retracted=-1.151;
+            public static double zero=0;
+        };
+        public enum right {
+            public static double extended=5.315;
+            public static double retracted=-1.172;
+            public static double zero=0;
+        };
+    }
     private static final double gearing = 9;
     private static final double kInPerSec = ((kNEOFreeSpeedRPM / gearing) * Math.PI * kDiameterIn / 60);
     private static double kRetractSpeed = -((kDesiredRetractSpeedInps / kInPerSec)
@@ -61,16 +73,17 @@ public Climber() {
         leftEncoder.setPosition(0);
         rightEncoder.setPositionConversionFactor(1 / gearing);
         rightEncoder.setPosition(0);
+
         SmartDashboard.putString("Left climber is", "Stopped");
         SmartDashboard.putString("Right climber is", "Stopped");
-        SmartDashboard.putNumber("kDesiredExtendSpeedInps", kDesiredExtendSpeedInps);
-        SmartDashboard.putNumber("kDesiredRetractSpeedInps", kDesiredRetractSpeedInps);
+        // SmartDashboard.putNumber("kDesiredExtendSpeedInps", kDesiredExtendSpeedInps);
+        // SmartDashboard.putNumber("kDesiredRetractSpeedInps", kDesiredRetractSpeedInps);
     }
 
     @Override
     public void periodic() {
-        SmartDashboard.putNumber("Left Climber Position", getLeftPosition());
-        SmartDashboard.putNumber("Right Climber Position", getRightPosition());
+        SmartDashboard.putNumber("L Climber Pos", leftEncoder.getPosition());
+        SmartDashboard.putNumber("R Climber Pos", rightEncoder.getPosition());
     }
 
     public void resetEncodersToExtended() {
@@ -155,44 +168,36 @@ public void stopRight(boolean interrupted) {
         stopRight();
     }
 
-    public double getLeftPosition() {
-        return leftEncoder.getPosition();
-    }
-
-    public double getRightPosition() {
-        return rightEncoder.getPosition();
-    }
-
     public boolean isLeftExtended() {
-        return getLeftPosition() >= extendPositionLeft;
+        return leftEncoder.getPosition() >= extendPositionLeft;
     }
 
     public boolean isRightExtended() {
-        return getRightPosition() >= extendPositionRight;
+        return rightEncoder.getPosition() >= extendPositionRight;
     }
 
     public boolean isLeftRetracted() {
-        return getLeftPosition() <= retractPositionLeft;
+        return leftEncoder.getPosition() <= retractPositionLeft;
     }
 
     public boolean isRightRetracted() {
-        return getRightPosition() <= retractPositionRight;
+        return rightEncoder.getPosition() <= retractPositionRight;
     }
 
     public boolean isRightResetExtended() {
-        return getRightPosition() >= 0;
+        return rightEncoder.getPosition() >= 0;
     }
 
     public boolean isLeftResetExtended() {
-        return getRightPosition() >= 0;
+        return rightEncoder.getPosition() >= 0;
     }
 
     public boolean isRightResetRetracted() {
-        return getRightPosition() <= 0;
+        return rightEncoder.getPosition() <= 0;
     }
 
     public boolean isLeftResetRetracted() {
-        return getRightPosition() <= 0;
+        return rightEncoder.getPosition() <= 0;
     }
 
 }

From db4e63a4f3968f26f17399f8ff4495fc9eb404f9 Mon Sep 17 00:00:00 2001
From: FriedLongJohns <81837862+FriedLongJohns@users.noreply.github.com>
Date: Sat, 10 Sep 2022 18:07:28 -0700
Subject: [PATCH 02/28] Rework Climber Subsystem

-Put all Encoder Positions and Motor speeds into enums
-Replaced all encoder set and motor speed set funcs with resetEncodersTo() and moveMotors() which use list lambdas to go fast and not check if statements
-replace all boolean funcs with isMotorExtended and isMotorRetracted

for all funcs using int argument called "motor", -1 left, 0 both, and 1 right motor

resetEncodersTo.*() -> resetEncodersTo(EncoderPos, int motor)

(?:slow)?(extend|retract).*() -> moveMotors(MotorSpeed, int motor)

stop.*() -> stopMotors(int motor)

is(Right|Left)(?:Reset)?(Retracted|Extended) -> isMotorExtended(int motor, boolean reset) and isMotorRetracted(int motor, boolean reset)
---
 .../team199/robot2022/subsystems/Climber.java | 212 +++++++-----------
 1 file changed, 86 insertions(+), 126 deletions(-)

diff --git a/src/main/java/org/team199/robot2022/subsystems/Climber.java b/src/main/java/org/team199/robot2022/subsystems/Climber.java
index f289c9c..894de93 100644
--- a/src/main/java/org/team199/robot2022/subsystems/Climber.java
+++ b/src/main/java/org/team199/robot2022/subsystems/Climber.java
@@ -26,23 +26,22 @@ public class Climber extends SubsystemBase {
 
     private static final boolean leftInverted = false;
 
-    private static final double extendPositionLeft = 5.317;
-    private static final double extendPositionRight = 5.315;
-
-    private static final double retractPositionLeft = -1.151;
-    private static final double retractPositionRight = -1.172;
-    public enum encoderResetPos {
-        public enum left {
-            public static double extended=5.317;
-            public static double retracted=-1.151;
-            public static double zero=0;
-        };
-        public enum right {
-            public static double extended=5.315;
-            public static double retracted=-1.172;
-            public static double zero=0;
-        };
-    }
+
+    private static final double extendLeft = 5.317;
+    private static final double extendRight = 5.315;
+    private static final double retractLeft = -1.151;
+    private static final double retractRight = -1.172;
+    private static final double zero = 0;
+
+    public static final enum EncoderPos{
+      extendLeft,
+      retractLeft
+      extendRight,
+      retractRight,
+      zero
+    };
+
+
     private static final double gearing = 9;
     private static final double kInPerSec = ((kNEOFreeSpeedRPM / gearing) * Math.PI * kDiameterIn / 60);
     private static double kRetractSpeed = -((kDesiredRetractSpeedInps / kInPerSec)
@@ -60,11 +59,33 @@ public enum right {
             + (kSlowVoltsToCounterTorque / 12)); // ~ -0.06151
     private static final double kSlowExtendSpeed = (kSlowDesiredExtendSpeedInps / kInPerSec); // ~0.30261
 
+    public static final enum MotorSpeed{
+      kSlowExtendSpeed,
+      kSlowRetractSpeed,
+      kExtendSpeed,
+      kRetractSpeed
+    }
+
+
     private final CANSparkMax left = MotorControllerFactory.createSparkMax(Constants.DrivePorts.kClimberLeft);
     private final CANSparkMax right = MotorControllerFactory.createSparkMax(Constants.DrivePorts.kClimberRight);
     private final RelativeEncoder leftEncoder = left.getEncoder();
     private final RelativeEncoder rightEncoder = right.getEncoder();
 
+
+    private final Consumer<void>[] setEncoder = {//faster array[](inp) than two if's + an else in a func
+      (EncoderPos pos) -> leftEncoder.setPosition(pos),
+      (EncoderPos pos) -> {leftEncoder.setPosition(pos);rightEncoder.setPosition(pos)},
+      (EncoderPos pos) -> rightEncoder.setPosition(pos),
+    };
+
+    private final Consumer<void>[] setMotor = {
+      (MotorSpeed speed) -> {left.set(speed);SmartDashboard.putString("Left Climber State", "Moving");},
+      (MotorSpeed speed) -> {left.set(speed);right.set(speed);SmartDashboard.putString("Left Climber State", "Moving");SmartDashboard.putString("Right Climber State", "Moving");},
+      (MotorSpeed speed) -> {right.set(speed);SmartDashboard.putString("Right Climber State", "Moving");},
+    }
+
+
     public Climber() {
         left.setInverted(leftInverted);
         right.setInverted(!leftInverted);
@@ -73,11 +94,10 @@ public Climber() {
         leftEncoder.setPosition(0);
         rightEncoder.setPositionConversionFactor(1 / gearing);
         rightEncoder.setPosition(0);
-
-        SmartDashboard.putString("Left climber is", "Stopped");
-        SmartDashboard.putString("Right climber is", "Stopped");
-        // SmartDashboard.putNumber("kDesiredExtendSpeedInps", kDesiredExtendSpeedInps);
-        // SmartDashboard.putNumber("kDesiredRetractSpeedInps", kDesiredRetractSpeedInps);
+        SmartDashboard.putString("Left Climber State", "Stop");
+        SmartDashboard.putString("Right Climber State", "Stop");
+        SmartDashboard.putNumber("kDesiredExtendSpeedInps", kDesiredExtendSpeedInps);
+        SmartDashboard.putNumber("kDesiredRetractSpeedInps", kDesiredRetractSpeedInps);
     }
 
     @Override
@@ -86,118 +106,58 @@ public void periodic() {
         SmartDashboard.putNumber("R Climber Pos", rightEncoder.getPosition());
     }
 
-    public void resetEncodersToExtended() {
-        leftEncoder.setPosition(extendPositionLeft);
-        rightEncoder.setPosition(extendPositionRight);
-    }
-
-    public void resetEncodersToRetracted() {
-        leftEncoder.setPosition(retractPositionLeft);
-        rightEncoder.setPosition(retractPositionRight);
-    }
-    public void resetEncodersToZero() {
-        leftEncoder.setPosition(0);
-        rightEncoder.setPosition(0);
-    }
-
-    public void extendLeft() {
-        left.set(kExtendSpeed);
-        SmartDashboard.putString("Left climber is", "Extending");
-    }
-
-    public void extendRight() {
-        right.set(kExtendSpeed);
-        SmartDashboard.putString("Right climber is", "Extending");
-    }
-
-    public void retractLeft() {
-        left.set(kRetractSpeed);
-        SmartDashboard.putString("Left climber is", "Retracting");
-    }
-
-    public void retractRight() {
-        right.set(kRetractSpeed);
-        SmartDashboard.putString("Right climber is", "Retracting");
-    }
-
-    public void slowExtendLeft() {
-        left.set(kSlowExtendSpeed);
-        SmartDashboard.putString("Left climber is", "Extending");
-    }
-
-    public void slowExtendRight() {
-        right.set(kSlowExtendSpeed);
-        SmartDashboard.putString("Right climber is", "Extending");
-    }
-    public void slowRetractLeft() {
-        left.set(kSlowRetractSpeed);
-        SmartDashboard.putString("Left climber is", "Retracting");
-    }
-
-    public void slowRetractRight() {
-        right.set(kSlowRetractSpeed);
-        SmartDashboard.putString("Right climber is", "Retracting");
+    public void resetEncodersTo(EncoderPos pos,int motor){//motor = -1 left or 1 right or 0 both
+      setEncoder[motor+1](pos);
     }
 
-    public void stop() {
-        left.set(0);
-        right.set(0);
-        SmartDashboard.putString("Left climber is", "Stopped");
-        SmartDashboard.putString("Right climber is", "Stopped");
+    public void moveMotors(MotorSpeed speed, int motor){//motor = -1 left or 1 right or 0 both
+      setMotor[motor+1](speed);
     }
 
-    public void stopLeft() {
+    public void stopMotors(int motor){//motor = -1 left or 1 right or 0 both
+      if (motor<1){
         left.set(0);
         SmartDashboard.putString("Left climber is", "Stopped");
-    }
-
-    public void stopRight() {
+      }
+      if (motor>-1){
         right.set(0);
         SmartDashboard.putString("Right climber is", "Stopped");
-    }
-
-    public void stop(boolean interrupted) {
-        stop();
-    }
-
-    public void stopLeft(boolean interrupted) {
-        stopLeft();
-    }
-
-    public void stopRight(boolean interrupted) {
-        stopRight();
-    }
-
-    public boolean isLeftExtended() {
-        return leftEncoder.getPosition() >= extendPositionLeft;
-    }
-
-    public boolean isRightExtended() {
-        return rightEncoder.getPosition() >= extendPositionRight;
-    }
-
-    public boolean isLeftRetracted() {
-        return leftEncoder.getPosition() <= retractPositionLeft;
-    }
-
-    public boolean isRightRetracted() {
-        return rightEncoder.getPosition() <= retractPositionRight;
-    }
-
-    public boolean isRightResetExtended() {
-        return rightEncoder.getPosition() >= 0;
-    }
-
-    public boolean isLeftResetExtended() {
-        return rightEncoder.getPosition() >= 0;
-    }
-
-    public boolean isRightResetRetracted() {
-        return rightEncoder.getPosition() <= 0;
-    }
-
-    public boolean isLeftResetRetracted() {
-        return rightEncoder.getPosition() <= 0;
+      }
+    }
+
+
+    public boolean isMotorExtended(int motor, boolean reset){//motor = -1 left or 1 right
+      if (motor==1){
+        if (!reset){
+          return getRightPosition() >= EncoderPos.extendRight;
+        }else{
+          return getRightPosition() >= 0;
+        }
+      }
+      if (motor==-1){
+        if (!reset){
+          return getLeftPosition() >= EncoderPos.extendLeft;
+        }else{
+          return getLeftPosition() >= 0;
+        }
+      }
+    }
+
+    public boolean isMotorRetracted(int motor, boolean reset){//motor = -1 left or 1 right
+      if (motor==1){
+        if (!reset){
+          return getRightPosition() <= EncoderPos.retractLeft;
+        }else{
+          return getRightPosition() <= 0;
+        }
+      }
+      if (motor==-1){
+        if (!reset){
+          return getLeftPosition() <= EncoderPos.retractLeft;
+        }else{
+          return getLeftPosition() <= 0;
+        }
+      }
     }
 
 }

From e7ac05340c7f9098d94177d2028251ac09243244 Mon Sep 17 00:00:00 2001
From: FriedLongJohns <81837862+FriedLongJohns@users.noreply.github.com>
Date: Sat, 10 Sep 2022 18:08:03 -0700
Subject: [PATCH 03/28] Use new climber functions and stop extending
 ParallelCommandGroup

---
 .../robot2022/commands/ExtendClimber.java     | 34 ++++++++--------
 .../commands/ResetAndExtendClimber.java       | 39 +++++++++++--------
 .../commands/ResetAndRetractClimber.java      | 37 ++++++++++--------
 .../robot2022/commands/RetractClimber.java    | 32 ++++++++-------
 4 files changed, 78 insertions(+), 64 deletions(-)

diff --git a/src/main/java/org/team199/robot2022/commands/ExtendClimber.java b/src/main/java/org/team199/robot2022/commands/ExtendClimber.java
index a7f6313..865d37a 100644
--- a/src/main/java/org/team199/robot2022/commands/ExtendClimber.java
+++ b/src/main/java/org/team199/robot2022/commands/ExtendClimber.java
@@ -9,24 +9,26 @@
 import edu.wpi.first.wpilibj2.command.FunctionalCommand;
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 
-public class ExtendClimber extends ParallelCommandGroup {
+public class ExtendClimber extends CommandBase {
+    private final Climber climber;
 
     public ExtendClimber(Climber climber) {
-        super(
-            new FunctionalCommand(
-                () -> {},
-                climber::extendLeft,
-                climber::stopLeft,
-                climber::isLeftExtended
-            ),
-            new FunctionalCommand(
-                () -> {},
-                climber::extendRight,
-                climber::stopRight,
-                climber::isRightExtended
-            )
-        );
-        addRequirements(climber);
+        addRequirements(this.climber = climber);
+    }
+
+    @Override
+    public void initialize() {
+        climber.moveMotors(climber.MotorSpeed.kExtendSpeed,0);
+    }
+
+    @Override
+    public boolean isFinished(){
+        return (isMotorExtended(-1,false) && isMotorExtended(1,false));
+    }
+
+    @Override
+    public void end(){
+        climber.stopMotors(0);
     }
 
 }
diff --git a/src/main/java/org/team199/robot2022/commands/ResetAndExtendClimber.java b/src/main/java/org/team199/robot2022/commands/ResetAndExtendClimber.java
index 43d9ac6..6ef19c4 100644
--- a/src/main/java/org/team199/robot2022/commands/ResetAndExtendClimber.java
+++ b/src/main/java/org/team199/robot2022/commands/ResetAndExtendClimber.java
@@ -4,23 +4,28 @@
 import edu.wpi.first.wpilibj2.command.FunctionalCommand;
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 
-public class ResetAndExtendClimber extends ParallelCommandGroup{
+public class ResetAndExtendClimber extends CommandBase {
+    private final Climber climber;
+    
     public ResetAndExtendClimber(Climber climber) {
-        super(
-            new FunctionalCommand(
-                climber::resetEncodersToRetracted,
-                climber::slowExtendLeft,
-                climber::stopLeft,
-                climber::isLeftResetExtended
-            ),
-            new FunctionalCommand(
-                climber::resetEncodersToRetracted,
-                climber::slowExtendRight,
-                climber::stopRight,
-                climber::isRightResetExtended
-            )
-        );
-        addRequirements(climber);
+        addRequirements(this.climber = climber);
     }
-    
+
+    @Override
+    public void initialize() {
+        climber.resetEncodersTo(climber.EncoderPos.retractLeft,-1);
+        climber.resetEncodersTo(climber.EncoderPos.retractRight,1);
+        climber.moveMotors(climber.MotorSpeed.kSlowExtendSpeed,0);
+    }
+
+    @Override
+    public boolean isFinished(){
+        return (isMotorExtended(-1,true) && isMotorExtended(1,true));
+    }
+
+    @Override
+    public void end(){
+        climber.stopMotors(0);
+    }
+
 }
diff --git a/src/main/java/org/team199/robot2022/commands/ResetAndRetractClimber.java b/src/main/java/org/team199/robot2022/commands/ResetAndRetractClimber.java
index 50a31ac..48bbf3c 100644
--- a/src/main/java/org/team199/robot2022/commands/ResetAndRetractClimber.java
+++ b/src/main/java/org/team199/robot2022/commands/ResetAndRetractClimber.java
@@ -5,22 +5,27 @@
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 
 public class ResetAndRetractClimber extends ParallelCommandGroup{
+    private final Climber climber;
+
     public ResetAndRetractClimber(Climber climber) {
-        super(
-            new FunctionalCommand(
-                climber::resetEncodersToExtended,
-                climber::slowRetractLeft,
-                climber::stopLeft,
-                climber::isLeftResetRetracted
-            ),
-            new FunctionalCommand(
-                climber::resetEncodersToExtended,
-                climber::slowRetractRight,
-                climber::stopRight,
-                climber::isRightResetRetracted
-            )
-        );
-        addRequirements(climber);
+        addRequirements(this.climber = climber);
+    }
+
+    @Override
+    public void initialize() {
+        climber.resetEncodersTo(climber.EncoderPos.extendtLeft,-1);
+        climber.resetEncodersTo(climber.EncoderPos.extendRight,1);
+        climber.moveMotors(climber.MotorSpeed.kSlowRetractSpeed,0);
+    }
+
+    @Override
+    public boolean isFinished(){
+        return (isMotorRetracted(-1,true) && isMotorRetracted(1,true));
+    }
+
+    @Override
+    public void end(){
+        climber.stopMotors(0);
     }
-    
+
 }
diff --git a/src/main/java/org/team199/robot2022/commands/RetractClimber.java b/src/main/java/org/team199/robot2022/commands/RetractClimber.java
index 7b9636f..534560f 100644
--- a/src/main/java/org/team199/robot2022/commands/RetractClimber.java
+++ b/src/main/java/org/team199/robot2022/commands/RetractClimber.java
@@ -10,23 +10,25 @@
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 
 public class RetractClimber extends ParallelCommandGroup {
+    private final Climber climber;
 
     public RetractClimber(Climber climber) {
-        super(
-            new FunctionalCommand(
-                () -> {},
-                climber::retractLeft,
-                climber::stopLeft,
-                climber::isLeftRetracted
-            ),
-            new FunctionalCommand(
-                () -> {},
-                climber::retractRight,
-                climber::stopRight,
-                climber::isRightRetracted
-            )
-        );
-        addRequirements(climber);
+        addRequirements(this.climber = climber);
+    }
+
+    @Override
+    public void initialize() {
+        climber.moveMotors(climber.MotorSpeed.kRetractSpeed,0);
+    }
+
+    @Override
+    public boolean isFinished(){
+        return (isMotorRetracted(-1,false) && isMotorRetracted(1,false));
+    }
+
+    @Override
+    public void end(){
+        climber.stopMotors(0);
     }
 
 }

From 4a9bc0e8edbc579448e368d9b40543f4992801d0 Mon Sep 17 00:00:00 2001
From: FriedLongJohns <81837862+FriedLongJohns@users.noreply.github.com>
Date: Fri, 16 Sep 2022 18:36:50 -0700
Subject: [PATCH 04/28] Remove Climber Reset

---
 .../commands/ResetAndExtendClimber.java       | 31 -------------------
 .../commands/ResetAndRetractClimber.java      | 31 -------------------
 2 files changed, 62 deletions(-)
 delete mode 100644 src/main/java/org/team199/robot2022/commands/ResetAndExtendClimber.java
 delete mode 100644 src/main/java/org/team199/robot2022/commands/ResetAndRetractClimber.java

diff --git a/src/main/java/org/team199/robot2022/commands/ResetAndExtendClimber.java b/src/main/java/org/team199/robot2022/commands/ResetAndExtendClimber.java
deleted file mode 100644
index 6ef19c4..0000000
--- a/src/main/java/org/team199/robot2022/commands/ResetAndExtendClimber.java
+++ /dev/null
@@ -1,31 +0,0 @@
-package org.team199.robot2022.commands;
-import org.team199.robot2022.subsystems.Climber;
-
-import edu.wpi.first.wpilibj2.command.FunctionalCommand;
-import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
-
-public class ResetAndExtendClimber extends CommandBase {
-    private final Climber climber;
-    
-    public ResetAndExtendClimber(Climber climber) {
-        addRequirements(this.climber = climber);
-    }
-
-    @Override
-    public void initialize() {
-        climber.resetEncodersTo(climber.EncoderPos.retractLeft,-1);
-        climber.resetEncodersTo(climber.EncoderPos.retractRight,1);
-        climber.moveMotors(climber.MotorSpeed.kSlowExtendSpeed,0);
-    }
-
-    @Override
-    public boolean isFinished(){
-        return (isMotorExtended(-1,true) && isMotorExtended(1,true));
-    }
-
-    @Override
-    public void end(){
-        climber.stopMotors(0);
-    }
-
-}
diff --git a/src/main/java/org/team199/robot2022/commands/ResetAndRetractClimber.java b/src/main/java/org/team199/robot2022/commands/ResetAndRetractClimber.java
deleted file mode 100644
index 48bbf3c..0000000
--- a/src/main/java/org/team199/robot2022/commands/ResetAndRetractClimber.java
+++ /dev/null
@@ -1,31 +0,0 @@
-package org.team199.robot2022.commands;
-import org.team199.robot2022.subsystems.Climber;
-
-import edu.wpi.first.wpilibj2.command.FunctionalCommand;
-import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
-
-public class ResetAndRetractClimber extends ParallelCommandGroup{
-    private final Climber climber;
-
-    public ResetAndRetractClimber(Climber climber) {
-        addRequirements(this.climber = climber);
-    }
-
-    @Override
-    public void initialize() {
-        climber.resetEncodersTo(climber.EncoderPos.extendtLeft,-1);
-        climber.resetEncodersTo(climber.EncoderPos.extendRight,1);
-        climber.moveMotors(climber.MotorSpeed.kSlowRetractSpeed,0);
-    }
-
-    @Override
-    public boolean isFinished(){
-        return (isMotorRetracted(-1,true) && isMotorRetracted(1,true));
-    }
-
-    @Override
-    public void end(){
-        climber.stopMotors(0);
-    }
-
-}

From 5e84e12758841bc6d1a581008cdf9258a69d9c0e Mon Sep 17 00:00:00 2001
From: FriedLongJohns <81837862+FriedLongJohns@users.noreply.github.com>
Date: Fri, 16 Sep 2022 18:38:31 -0700
Subject: [PATCH 05/28] Use more tuples + bugfixes

+Motor selection tuple
+getMotorPos now maps to a lambda array
=Rework isMotor(Extended|Retracted) to not  use reset boolean anymore and operate on a "true until false" basis
---
 .../robot2022/commands/ExtendClimber.java     |  6 +-
 .../robot2022/commands/RetractClimber.java    |  6 +-
 .../team199/robot2022/subsystems/Climber.java | 62 ++++++++++---------
 3 files changed, 38 insertions(+), 36 deletions(-)

diff --git a/src/main/java/org/team199/robot2022/commands/ExtendClimber.java b/src/main/java/org/team199/robot2022/commands/ExtendClimber.java
index 865d37a..4f5b1a2 100644
--- a/src/main/java/org/team199/robot2022/commands/ExtendClimber.java
+++ b/src/main/java/org/team199/robot2022/commands/ExtendClimber.java
@@ -18,17 +18,17 @@ public ExtendClimber(Climber climber) {
 
     @Override
     public void initialize() {
-        climber.moveMotors(climber.MotorSpeed.kExtendSpeed,0);
+        climber.moveMotors(climber.MotorSpeed.kExtendSpeed,climber.Motor.both);
     }
 
     @Override
     public boolean isFinished(){
-        return (isMotorExtended(-1,false) && isMotorExtended(1,false));
+        return isMotorExtended(climber.Motor.both);
     }
 
     @Override
     public void end(){
-        climber.stopMotors(0);
+        climber.stopMotors(climber.Motor.both);
     }
 
 }
diff --git a/src/main/java/org/team199/robot2022/commands/RetractClimber.java b/src/main/java/org/team199/robot2022/commands/RetractClimber.java
index 534560f..1d8abf2 100644
--- a/src/main/java/org/team199/robot2022/commands/RetractClimber.java
+++ b/src/main/java/org/team199/robot2022/commands/RetractClimber.java
@@ -18,17 +18,17 @@ public RetractClimber(Climber climber) {
 
     @Override
     public void initialize() {
-        climber.moveMotors(climber.MotorSpeed.kRetractSpeed,0);
+        climber.moveMotors(climber.MotorSpeed.kRetractSpeed,climber.Motor.both);
     }
 
     @Override
     public boolean isFinished(){
-        return (isMotorRetracted(-1,false) && isMotorRetracted(1,false));
+        return isMotorRetracted(climber.Motor.both);
     }
 
     @Override
     public void end(){
-        climber.stopMotors(0);
+        climber.stopMotors(climber.Motor.both);
     }
 
 }
diff --git a/src/main/java/org/team199/robot2022/subsystems/Climber.java b/src/main/java/org/team199/robot2022/subsystems/Climber.java
index 894de93..d2c4776 100644
--- a/src/main/java/org/team199/robot2022/subsystems/Climber.java
+++ b/src/main/java/org/team199/robot2022/subsystems/Climber.java
@@ -66,6 +66,15 @@ public static final enum MotorSpeed{
       kRetractSpeed
     }
 
+    private static final int left = -1;
+    private static final int both = 0;
+    private static final int right = 1;
+    public static final enum Motor{
+      left,
+      right,
+      both,
+    }
+
 
     private final CANSparkMax left = MotorControllerFactory.createSparkMax(Constants.DrivePorts.kClimberLeft);
     private final CANSparkMax right = MotorControllerFactory.createSparkMax(Constants.DrivePorts.kClimberRight);
@@ -83,7 +92,12 @@ public static final enum MotorSpeed{
       (MotorSpeed speed) -> {left.set(speed);SmartDashboard.putString("Left Climber State", "Moving");},
       (MotorSpeed speed) -> {left.set(speed);right.set(speed);SmartDashboard.putString("Left Climber State", "Moving");SmartDashboard.putString("Right Climber State", "Moving");},
       (MotorSpeed speed) -> {right.set(speed);SmartDashboard.putString("Right Climber State", "Moving");},
-    }
+    };
+
+    private final Consumer<double>[] getMotorPos = {
+      () -> leftEncoder.getPosition(),
+      () -> rightEncoder.getPosition(),
+    };
 
 
     public Climber() {
@@ -106,15 +120,15 @@ public void periodic() {
         SmartDashboard.putNumber("R Climber Pos", rightEncoder.getPosition());
     }
 
-    public void resetEncodersTo(EncoderPos pos,int motor){//motor = -1 left or 1 right or 0 both
+    public void resetEncodersTo(EncoderPos pos,Motor motor){//motor = -1 left or 1 right or 0 both
       setEncoder[motor+1](pos);
     }
 
-    public void moveMotors(MotorSpeed speed, int motor){//motor = -1 left or 1 right or 0 both
+    public void moveMotors(MotorSpeed speed, Motor motor){//motor = -1 left or 1 right or 0 both
       setMotor[motor+1](speed);
     }
 
-    public void stopMotors(int motor){//motor = -1 left or 1 right or 0 both
+    public void stopMotors(Motor motor){//motor = -1 left or 1 right or 0 both
       if (motor<1){
         left.set(0);
         SmartDashboard.putString("Left climber is", "Stopped");
@@ -126,38 +140,26 @@ public void stopMotors(int motor){//motor = -1 left or 1 right or 0 both
     }
 
 
-    public boolean isMotorExtended(int motor, boolean reset){//motor = -1 left or 1 right
-      if (motor==1){
-        if (!reset){
-          return getRightPosition() >= EncoderPos.extendRight;
-        }else{
-          return getRightPosition() >= 0;
-        }
+    public boolean isMotorExtended(Motor motor){//motor = -1 left or 1 right or 0 both
+      if (motor<1 && getMotorPos[0]() < EncoderPos.extendLeft){
+        //getLeftPosition() >= EncoderPos.extendLeft;
+        return false;
       }
-      if (motor==-1){
-        if (!reset){
-          return getLeftPosition() >= EncoderPos.extendLeft;
-        }else{
-          return getLeftPosition() >= 0;
-        }
+      if (motor>-1 && getMotorPos[1]() < EncoderPos.extendRight){
+        //return getRightPosition() >= EncoderPos.extendRight;
+        return false;
       }
+      return true;
     }
 
-    public boolean isMotorRetracted(int motor, boolean reset){//motor = -1 left or 1 right
-      if (motor==1){
-        if (!reset){
-          return getRightPosition() <= EncoderPos.retractLeft;
-        }else{
-          return getRightPosition() <= 0;
-        }
+    public boolean isMotorRetracted(Motor motor){//motor = -1 left or 1 right or 0 both
+      if (motor<1 && getMotorPos[0]() > EncoderPos.retractLeft){
+        return false;
       }
-      if (motor==-1){
-        if (!reset){
-          return getLeftPosition() <= EncoderPos.retractLeft;
-        }else{
-          return getLeftPosition() <= 0;
-        }
+      if (motor>-1 && getMotorPos[1]() > EncoderPos.retractRight){
+        return false;
       }
+      return true;
     }
 
 }

From 26cfa7fb387a314bb0fa4df66236341edf3d56b1 Mon Sep 17 00:00:00 2001
From: FriedLongJohns <81837862+FriedLongJohns@users.noreply.github.com>
Date: Fri, 16 Sep 2022 19:20:51 -0700
Subject: [PATCH 06/28] Lots of bugfixes + use constants instead of enums

---
 .../robot2022/commands/ExtendClimber.java     |  6 +--
 .../robot2022/commands/RetractClimber.java    |  6 +--
 .../team199/robot2022/subsystems/Climber.java | 47 +++++++++----------
 3 files changed, 28 insertions(+), 31 deletions(-)

diff --git a/src/main/java/org/team199/robot2022/commands/ExtendClimber.java b/src/main/java/org/team199/robot2022/commands/ExtendClimber.java
index 4f5b1a2..9f94114 100644
--- a/src/main/java/org/team199/robot2022/commands/ExtendClimber.java
+++ b/src/main/java/org/team199/robot2022/commands/ExtendClimber.java
@@ -18,17 +18,17 @@ public ExtendClimber(Climber climber) {
 
     @Override
     public void initialize() {
-        climber.moveMotors(climber.MotorSpeed.kExtendSpeed,climber.Motor.both);
+        climber.moveMotors(climber.MotorSpeed.kExtendSpeed,climber.bothMotors);
     }
 
     @Override
     public boolean isFinished(){
-        return isMotorExtended(climber.Motor.both);
+        return isMotorExtended(climber.bothMotors);
     }
 
     @Override
     public void end(){
-        climber.stopMotors(climber.Motor.both);
+        climber.stopMotors(climber.bothMotors);
     }
 
 }
diff --git a/src/main/java/org/team199/robot2022/commands/RetractClimber.java b/src/main/java/org/team199/robot2022/commands/RetractClimber.java
index 1d8abf2..df67ea2 100644
--- a/src/main/java/org/team199/robot2022/commands/RetractClimber.java
+++ b/src/main/java/org/team199/robot2022/commands/RetractClimber.java
@@ -18,17 +18,17 @@ public RetractClimber(Climber climber) {
 
     @Override
     public void initialize() {
-        climber.moveMotors(climber.MotorSpeed.kRetractSpeed,climber.Motor.both);
+        climber.moveMotors(climber.MotorSpeed.kRetractSpeed,climber.bothMotors);
     }
 
     @Override
     public boolean isFinished(){
-        return isMotorRetracted(climber.Motor.both);
+        return isMotorRetracted(climber.bothMotors);
     }
 
     @Override
     public void end(){
-        climber.stopMotors(climber.Motor.both);
+        climber.stopMotors(climber.bothMotors);
     }
 
 }
diff --git a/src/main/java/org/team199/robot2022/subsystems/Climber.java b/src/main/java/org/team199/robot2022/subsystems/Climber.java
index d2c4776..c7d4dbc 100644
--- a/src/main/java/org/team199/robot2022/subsystems/Climber.java
+++ b/src/main/java/org/team199/robot2022/subsystems/Climber.java
@@ -33,12 +33,12 @@ public class Climber extends SubsystemBase {
     private static final double retractRight = -1.172;
     private static final double zero = 0;
 
-    public static final enum EncoderPos{
+    public static enum EncoderPos{
       extendLeft,
-      retractLeft
+      retractLeft,
       extendRight,
       retractRight,
-      zero
+      zero,
     };
 
 
@@ -59,21 +59,16 @@ public static final enum EncoderPos{
             + (kSlowVoltsToCounterTorque / 12)); // ~ -0.06151
     private static final double kSlowExtendSpeed = (kSlowDesiredExtendSpeedInps / kInPerSec); // ~0.30261
 
-    public static final enum MotorSpeed{
+    public static enum MotorSpeed{
       kSlowExtendSpeed,
       kSlowRetractSpeed,
       kExtendSpeed,
-      kRetractSpeed
+      kRetractSpeed,
     }
 
-    private static final int left = -1;
-    private static final int both = 0;
-    private static final int right = 1;
-    public static final enum Motor{
-      left,
-      right,
-      both,
-    }
+    private static final int leftMotor = -1;
+    private static final int bothMotors = 0;
+    private static final int rightMotor = 1;
 
 
     private final CANSparkMax left = MotorControllerFactory.createSparkMax(Constants.DrivePorts.kClimberLeft);
@@ -84,7 +79,7 @@ public static final enum Motor{
 
     private final Consumer<void>[] setEncoder = {//faster array[](inp) than two if's + an else in a func
       (EncoderPos pos) -> leftEncoder.setPosition(pos),
-      (EncoderPos pos) -> {leftEncoder.setPosition(pos);rightEncoder.setPosition(pos)},
+      (EncoderPos pos) -> {leftEncoder.setPosition(pos);rightEncoder.setPosition(pos);},
       (EncoderPos pos) -> rightEncoder.setPosition(pos),
     };
 
@@ -120,15 +115,17 @@ public void periodic() {
         SmartDashboard.putNumber("R Climber Pos", rightEncoder.getPosition());
     }
 
-    public void resetEncodersTo(EncoderPos pos,Motor motor){//motor = -1 left or 1 right or 0 both
-      setEncoder[motor+1](pos);
+    public void resetEncodersTo(EncoderPos pos,int motor){//motor = -1 left or 1 right or 0 both
+			setEncoder[motor+1].apply(pos);
+//      Consumer func = setEncoder[motor+1];
+//			func(pos);
     }
 
-    public void moveMotors(MotorSpeed speed, Motor motor){//motor = -1 left or 1 right or 0 both
-      setMotor[motor+1](speed);
+    public void moveMotors(MotorSpeed speed, int motor){//motor = -1 left or 1 right or 0 both
+      setMotor[motor+1].apply(speed);
     }
 
-    public void stopMotors(Motor motor){//motor = -1 left or 1 right or 0 both
+    public void stopMotors(int motor){//motor = -1 left or 1 right or 0 both
       if (motor<1){
         left.set(0);
         SmartDashboard.putString("Left climber is", "Stopped");
@@ -140,23 +137,23 @@ public void stopMotors(Motor motor){//motor = -1 left or 1 right or 0 both
     }
 
 
-    public boolean isMotorExtended(Motor motor){//motor = -1 left or 1 right or 0 both
-      if (motor<1 && getMotorPos[0]() < EncoderPos.extendLeft){
+    public boolean isMotorExtended(int motor){//motor = -1 left or 1 right or 0 both
+      if (motor<1 && getMotorPos[0].apply() < EncoderPos.extendLeft){
         //getLeftPosition() >= EncoderPos.extendLeft;
         return false;
       }
-      if (motor>-1 && getMotorPos[1]() < EncoderPos.extendRight){
+      if (motor>-1 && getMotorPos[1].apply() < EncoderPos.extendRight){
         //return getRightPosition() >= EncoderPos.extendRight;
         return false;
       }
       return true;
     }
 
-    public boolean isMotorRetracted(Motor motor){//motor = -1 left or 1 right or 0 both
-      if (motor<1 && getMotorPos[0]() > EncoderPos.retractLeft){
+    public boolean isMotorRetracted(int motor){//motor = -1 left or 1 right or 0 both
+      if (motor<1 && getMotorPos[0].apply() > EncoderPos.retractLeft){
         return false;
       }
-      if (motor>-1 && getMotorPos[1]() > EncoderPos.retractRight){
+      if (motor>-1 && getMotorPos[1].apply() > EncoderPos.retractRight){
         return false;
       }
       return true;

From 5619f230b410da8b9699be3359a0b375f947d4fb Mon Sep 17 00:00:00 2001
From: FriedLongJohns <81837862+FriedLongJohns@users.noreply.github.com>
Date: Sun, 18 Sep 2022 16:08:13 -0700
Subject: [PATCH 07/28] edit comments

---
 .../team199/robot2022/subsystems/Climber.java | 19 ++++++++-----------
 1 file changed, 8 insertions(+), 11 deletions(-)

diff --git a/src/main/java/org/team199/robot2022/subsystems/Climber.java b/src/main/java/org/team199/robot2022/subsystems/Climber.java
index c7d4dbc..7ed181e 100644
--- a/src/main/java/org/team199/robot2022/subsystems/Climber.java
+++ b/src/main/java/org/team199/robot2022/subsystems/Climber.java
@@ -66,7 +66,7 @@ public static enum MotorSpeed{
       kRetractSpeed,
     }
 
-    private static final int leftMotor = -1;
+    private static final int leftMotor = -1;//because someone requested this and there's no point slapping them in an enum
     private static final int bothMotors = 0;
     private static final int rightMotor = 1;
 
@@ -77,7 +77,7 @@ public static enum MotorSpeed{
     private final RelativeEncoder rightEncoder = right.getEncoder();
 
 
-    private final Consumer<void>[] setEncoder = {//faster array[](inp) than two if's + an else in a func
+    private final Consumer<void>[] setEncoder = {//faster to array[](inp) than a bunch of if's in a func
       (EncoderPos pos) -> leftEncoder.setPosition(pos),
       (EncoderPos pos) -> {leftEncoder.setPosition(pos);rightEncoder.setPosition(pos);},
       (EncoderPos pos) -> rightEncoder.setPosition(pos),
@@ -115,17 +115,16 @@ public void periodic() {
         SmartDashboard.putNumber("R Climber Pos", rightEncoder.getPosition());
     }
 
-    public void resetEncodersTo(EncoderPos pos,int motor){//motor = -1 left or 1 right or 0 both
+    //motor = -1 left or 1 right or 0 both
+    public void resetEncodersTo(EncoderPos pos,int motor){
 			setEncoder[motor+1].apply(pos);
-//      Consumer func = setEncoder[motor+1];
-//			func(pos);
     }
 
-    public void moveMotors(MotorSpeed speed, int motor){//motor = -1 left or 1 right or 0 both
+    public void moveMotors(MotorSpeed speed, int motor){
       setMotor[motor+1].apply(speed);
     }
 
-    public void stopMotors(int motor){//motor = -1 left or 1 right or 0 both
+    public void stopMotors(int motor){
       if (motor<1){
         left.set(0);
         SmartDashboard.putString("Left climber is", "Stopped");
@@ -137,19 +136,17 @@ public void stopMotors(int motor){//motor = -1 left or 1 right or 0 both
     }
 
 
-    public boolean isMotorExtended(int motor){//motor = -1 left or 1 right or 0 both
+    public boolean isMotorExtended(int motor){//is the motor(s) extended?
       if (motor<1 && getMotorPos[0].apply() < EncoderPos.extendLeft){
-        //getLeftPosition() >= EncoderPos.extendLeft;
         return false;
       }
       if (motor>-1 && getMotorPos[1].apply() < EncoderPos.extendRight){
-        //return getRightPosition() >= EncoderPos.extendRight;
         return false;
       }
       return true;
     }
 
-    public boolean isMotorRetracted(int motor){//motor = -1 left or 1 right or 0 both
+    public boolean isMotorRetracted(int motor){//is the motor(s) retracted?
       if (motor<1 && getMotorPos[0].apply() > EncoderPos.retractLeft){
         return false;
       }

From 7c5390af656750ade067b7868a0af36f9184a76f Mon Sep 17 00:00:00 2001
From: FriedLongJohns <81837862+FriedLongJohns@users.noreply.github.com>
Date: Sun, 18 Sep 2022 20:27:33 -0700
Subject: [PATCH 08/28] Fix ALL the bugs (it builds now)

-Fix enum system in Climber.java to actually work as intended by using dictionaries.
-Go fix all the uses of Climber.java to also work correctly
-It builds now

Probably still horribly buggy
---
 .../org/team199/robot2022/RobotContainer.java |  17 ++-
 .../robot2022/commands/ExtendClimber.java     |  12 +--
 .../robot2022/commands/RetractClimber.java    |  14 +--
 .../team199/robot2022/subsystems/Climber.java | 100 +++++++-----------
 4 files changed, 55 insertions(+), 88 deletions(-)

diff --git a/src/main/java/org/team199/robot2022/RobotContainer.java b/src/main/java/org/team199/robot2022/RobotContainer.java
index b1125a5..5b64ece 100644
--- a/src/main/java/org/team199/robot2022/RobotContainer.java
+++ b/src/main/java/org/team199/robot2022/RobotContainer.java
@@ -11,8 +11,6 @@
 import org.team199.robot2022.commands.PassiveAutomaticIntake;
 import org.team199.robot2022.commands.PassiveManualIntake;
 import org.team199.robot2022.commands.Regurgitate;
-import org.team199.robot2022.commands.ResetAndExtendClimber;
-import org.team199.robot2022.commands.ResetAndRetractClimber;
 import org.team199.robot2022.commands.RetractClimber;
 import org.team199.robot2022.commands.Shoot;
 import org.team199.robot2022.commands.TeleopDrive;
@@ -60,7 +58,7 @@ public class RobotContainer {
 
   public final DigitalInput[] autoSelectors;
   public final AutoPath[] autoPaths;
-  
+
 
   /**
    * The container for the robot. Contains subsystems, OI devices, and commands.
@@ -126,17 +124,14 @@ private void configureButtonBindingsLeftJoy() {
     new JoystickButton(leftJoy, Constants.OI.LeftJoy.manualAddPort).whenPressed(new InstantCommand(intakeFeeder::manualAdd));
     new JoystickButton(leftJoy, Constants.OI.LeftJoy.manualSubtractPort).whenPressed(new InstantCommand(intakeFeeder::manualSub));
     new JoystickButton(leftJoy, Constants.OI.LeftJoy.overridePort).whenPressed(new InstantCommand(intakeFeeder::override));
-    new JoystickButton(leftJoy, Constants.OI.LeftJoy.resetAndExtendClimberPort).whenPressed(new ResetAndExtendClimber(climber));
-    new JoystickButton(leftJoy, Constants.OI.LeftJoy.resetAndRetractClimberPort).whenPressed(new ResetAndRetractClimber(climber));
-    new JoystickButton(leftJoy, Constants.OI.LeftJoy.resetClimberEncoders). whenPressed(new InstantCommand(climber::resetEncodersToZero));
   }
 
   private void configureButtonBindingsRightJoy() {
     new JoystickButton(rightJoy, Constants.OI.RightJoy.shootPort).whenPressed(new Shoot(intakeFeeder, shooter));
-    new JoystickButton(rightJoy, Constants.OI.RightJoy.slowExtendLeftClimberPort).whileHeld(new InstantCommand(climber::slowExtendLeft)).whenReleased(new InstantCommand(climber::stopLeft));
-    new JoystickButton(rightJoy, Constants.OI.RightJoy.slowRetractLeftClimberPort).whileHeld(new InstantCommand(climber::slowRetractLeft)).whenReleased(new InstantCommand(climber::stopLeft));
-    new JoystickButton(rightJoy, Constants.OI.RightJoy.slowExtendRightClimberPort).whileHeld(new InstantCommand(climber::slowExtendRight)).whenReleased(new InstantCommand(climber::stopRight));
-    new JoystickButton(rightJoy, Constants.OI.RightJoy.slowRetractRightClimberPort).whileHeld(new InstantCommand(climber::slowRetractRight)).whenReleased(new InstantCommand(climber::stopRight));
+    new JoystickButton(rightJoy, Constants.OI.RightJoy.slowExtendLeftClimberPort).whileHeld(new InstantCommand(()->climber.moveMotors(Climber.MotorSpeed.slowExtend,climber.leftMotor))).whenReleased(new InstantCommand(()->climber.stopMotors(climber.leftMotor)));
+    new JoystickButton(rightJoy, Constants.OI.RightJoy.slowRetractLeftClimberPort).whileHeld(new InstantCommand(()->climber.moveMotors(Climber.MotorSpeed.slowRetract,climber.leftMotor))).whenReleased(new InstantCommand(()->climber.stopMotors(climber.leftMotor)));
+    new JoystickButton(rightJoy, Constants.OI.RightJoy.slowExtendRightClimberPort).whileHeld(new InstantCommand(()->climber.moveMotors(Climber.MotorSpeed.slowExtend,climber.rightMotor))).whenReleased(new InstantCommand(()->climber.stopMotors(climber.rightMotor)));
+    new JoystickButton(rightJoy, Constants.OI.RightJoy.slowRetractRightClimberPort).whileHeld(new InstantCommand(()->climber.moveMotors(Climber.MotorSpeed.slowRetract,climber.rightMotor))).whenReleased(new InstantCommand(()->climber.stopMotors(climber.rightMotor)));
   }
 
   private void configureButtonBindingsController() {
@@ -197,7 +192,7 @@ private double getStickValue(Constants.OI.StickType stick, Constants.OI.StickDir
 
   /**
    * Processes an input from the joystick into a value between -1 and 1
-   * 
+   *
    * @param value The value to be processed.
    * @return The processed value.
    */
diff --git a/src/main/java/org/team199/robot2022/commands/ExtendClimber.java b/src/main/java/org/team199/robot2022/commands/ExtendClimber.java
index 9f94114..b34aeaf 100644
--- a/src/main/java/org/team199/robot2022/commands/ExtendClimber.java
+++ b/src/main/java/org/team199/robot2022/commands/ExtendClimber.java
@@ -6,27 +6,23 @@
 
 import org.team199.robot2022.subsystems.Climber;
 
-import edu.wpi.first.wpilibj2.command.FunctionalCommand;
-import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+import edu.wpi.first.wpilibj2.command.CommandBase;
 
 public class ExtendClimber extends CommandBase {
-    private final Climber climber;
+    private Climber climber;
 
     public ExtendClimber(Climber climber) {
         addRequirements(this.climber = climber);
     }
 
-    @Override
     public void initialize() {
-        climber.moveMotors(climber.MotorSpeed.kExtendSpeed,climber.bothMotors);
+        climber.moveMotors(Climber.MotorSpeed.extend,climber.bothMotors);
     }
 
-    @Override
     public boolean isFinished(){
-        return isMotorExtended(climber.bothMotors);
+        return climber.isMotorExtended(climber.bothMotors);
     }
 
-    @Override
     public void end(){
         climber.stopMotors(climber.bothMotors);
     }
diff --git a/src/main/java/org/team199/robot2022/commands/RetractClimber.java b/src/main/java/org/team199/robot2022/commands/RetractClimber.java
index df67ea2..9e4ade6 100644
--- a/src/main/java/org/team199/robot2022/commands/RetractClimber.java
+++ b/src/main/java/org/team199/robot2022/commands/RetractClimber.java
@@ -6,27 +6,23 @@
 
 import org.team199.robot2022.subsystems.Climber;
 
-import edu.wpi.first.wpilibj2.command.FunctionalCommand;
-import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+import edu.wpi.first.wpilibj2.command.CommandBase;
 
-public class RetractClimber extends ParallelCommandGroup {
-    private final Climber climber;
+public class RetractClimber extends CommandBase {
+    private Climber climber;
 
     public RetractClimber(Climber climber) {
         addRequirements(this.climber = climber);
     }
 
-    @Override
     public void initialize() {
-        climber.moveMotors(climber.MotorSpeed.kRetractSpeed,climber.bothMotors);
+        climber.moveMotors(Climber.MotorSpeed.retract,climber.bothMotors);
     }
 
-    @Override
     public boolean isFinished(){
-        return isMotorRetracted(climber.bothMotors);
+        return climber.isMotorRetracted(climber.bothMotors);
     }
 
-    @Override
     public void end(){
         climber.stopMotors(climber.bothMotors);
     }
diff --git a/src/main/java/org/team199/robot2022/subsystems/Climber.java b/src/main/java/org/team199/robot2022/subsystems/Climber.java
index 7ed181e..157006b 100644
--- a/src/main/java/org/team199/robot2022/subsystems/Climber.java
+++ b/src/main/java/org/team199/robot2022/subsystems/Climber.java
@@ -9,6 +9,9 @@
 import frc.robot.lib.MotorControllerFactory;
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.RelativeEncoder;
+import java.util.function.Consumer;
+import java.util.Hashtable;
+import java.util.Dictionary;
 
 import org.team199.robot2022.Constants;
 
@@ -18,57 +21,29 @@ public class Climber extends SubsystemBase {
     private static final double kDiameterIn = 1;
     private static final double kDesiredRetractSpeedInps = 1;
     private static final double kDesiredExtendSpeedInps = 6;
-
-    // Torque is 2 * 9 * 0.5 = 9
-    // Torque on the motor is Torque / ( gearing = 9 ) = 1
-
-    private static final double kVoltsToCounterTorque = 10.5;
-
     private static final boolean leftInverted = false;
 
-
-    private static final double extendLeft = 5.317;
-    private static final double extendRight = 5.315;
-    private static final double retractLeft = -1.151;
-    private static final double retractRight = -1.172;
-    private static final double zero = 0;
-
-    public static enum EncoderPos{
-      extendLeft,
-      retractLeft,
-      extendRight,
-      retractRight,
-      zero,
-    };
-
-
     private static final double gearing = 9;
     private static final double kInPerSec = ((kNEOFreeSpeedRPM / gearing) * Math.PI * kDiameterIn / 60);
-    private static double kRetractSpeed = -((kDesiredRetractSpeedInps / kInPerSec)
-            + (kVoltsToCounterTorque / 12)); // ~ -0.06151
-    private static double kExtendSpeed = (kDesiredExtendSpeedInps / kInPerSec); // ~0.30261
-
     private static final double kSlowDesiredRetractSpeedInps = 1;
     private static final double kSlowDesiredExtendSpeedInps = 1;
 
     // Torque is 2 * 9 * 0.5 = 9
     // Torque on the motor is Torque / ( gearing = 9 ) = 1
-
+    private static final double kVoltsToCounterTorque = 10.5;
     private static final double kSlowVoltsToCounterTorque = (1.1D / 32) * 12;
-    private static final double kSlowRetractSpeed = -((kSlowDesiredRetractSpeedInps / kInPerSec)
-            + (kSlowVoltsToCounterTorque / 12)); // ~ -0.06151
-    private static final double kSlowExtendSpeed = (kSlowDesiredExtendSpeedInps / kInPerSec); // ~0.30261
-
-    public static enum MotorSpeed{
-      kSlowExtendSpeed,
-      kSlowRetractSpeed,
-      kExtendSpeed,
-      kRetractSpeed,
-    }
 
-    private static final int leftMotor = -1;//because someone requested this and there's no point slapping them in an enum
-    private static final int bothMotors = 0;
-    private static final int rightMotor = 1;
+    public static enum MotorSpeed{retract,extend,slowRetract,slowExtend;};
+    private static Dictionary dMotorSpeed = new Hashtable();//given values in constructor
+
+
+    public static enum EncoderPos{extendLeft,extendRight,retractLeft,retractRight,zero;};
+    private static Dictionary dEncoderPos = new Hashtable();//given values in constructor
+
+
+    public static final int leftMotor = -1;//because someone requested this and there's no point slapping them in an enum
+    public static final int bothMotors = 0;
+    public static final int rightMotor = 1;
 
 
     private final CANSparkMax left = MotorControllerFactory.createSparkMax(Constants.DrivePorts.kClimberLeft);
@@ -77,24 +52,18 @@ public static enum MotorSpeed{
     private final RelativeEncoder rightEncoder = right.getEncoder();
 
 
-    private final Consumer<void>[] setEncoder = {//faster to array[](inp) than a bunch of if's in a func
-      (EncoderPos pos) -> leftEncoder.setPosition(pos),
-      (EncoderPos pos) -> {leftEncoder.setPosition(pos);rightEncoder.setPosition(pos);},
-      (EncoderPos pos) -> rightEncoder.setPosition(pos),
-    };
-
-    private final Consumer<void>[] setMotor = {
-      (MotorSpeed speed) -> {left.set(speed);SmartDashboard.putString("Left Climber State", "Moving");},
-      (MotorSpeed speed) -> {left.set(speed);right.set(speed);SmartDashboard.putString("Left Climber State", "Moving");SmartDashboard.putString("Right Climber State", "Moving");},
-      (MotorSpeed speed) -> {right.set(speed);SmartDashboard.putString("Right Climber State", "Moving");},
+    private final Consumer[] setEncoder = new Consumer[]{//faster to array[](inp) than a bunch of if's in a func
+      (pos) -> leftEncoder.setPosition((double) pos),
+      (pos) -> {leftEncoder.setPosition((double) pos);rightEncoder.setPosition((double) pos);},
+      (pos) -> rightEncoder.setPosition((double) pos),
     };
 
-    private final Consumer<double>[] getMotorPos = {
-      () -> leftEncoder.getPosition(),
-      () -> rightEncoder.getPosition(),
+    private final Consumer[] setMotor = new Consumer[]{
+      (speed) -> {left.set((double) speed);SmartDashboard.putString("Left Climber State", "Moving");},
+      (speed) -> {left.set((double) speed);right.set((double) speed);SmartDashboard.putString("Left Climber State", "Moving");SmartDashboard.putString("Right Climber State", "Moving");},
+      (speed) -> {right.set((double) speed);SmartDashboard.putString("Right Climber State", "Moving");},
     };
 
-
     public Climber() {
         left.setInverted(leftInverted);
         right.setInverted(!leftInverted);
@@ -107,6 +76,17 @@ public Climber() {
         SmartDashboard.putString("Right Climber State", "Stop");
         SmartDashboard.putNumber("kDesiredExtendSpeedInps", kDesiredExtendSpeedInps);
         SmartDashboard.putNumber("kDesiredRetractSpeedInps", kDesiredRetractSpeedInps);
+
+        dEncoderPos.put(EncoderPos.extendRight, 5.315);
+        dEncoderPos.put(EncoderPos.retractLeft, -1.151);
+        dEncoderPos.put(EncoderPos.extendLeft, -5.317);
+        dEncoderPos.put(EncoderPos.retractRight, -1.172);
+        dEncoderPos.put(EncoderPos.zero, 0.0);
+
+        dMotorSpeed.put(MotorSpeed.retract, -((kDesiredRetractSpeedInps / kInPerSec) + (kVoltsToCounterTorque / 12))); // ~ -0.06151
+        dMotorSpeed.put(MotorSpeed.extend, (kDesiredExtendSpeedInps / kInPerSec)); // ~0.30261
+        dMotorSpeed.put(MotorSpeed.slowRetract, -((kSlowDesiredRetractSpeedInps / kInPerSec) + (kSlowVoltsToCounterTorque / 12))); // ~ -0.06151
+        dMotorSpeed.put(MotorSpeed.slowExtend, (kSlowDesiredExtendSpeedInps / kInPerSec)); // ~0.30261
     }
 
     @Override
@@ -117,11 +97,11 @@ public void periodic() {
 
     //motor = -1 left or 1 right or 0 both
     public void resetEncodersTo(EncoderPos pos,int motor){
-			setEncoder[motor+1].apply(pos);
+			setEncoder[motor+1].accept(dEncoderPos.get(pos));
     }
 
     public void moveMotors(MotorSpeed speed, int motor){
-      setMotor[motor+1].apply(speed);
+      setMotor[motor+1].accept(dMotorSpeed.get(speed));
     }
 
     public void stopMotors(int motor){
@@ -137,20 +117,20 @@ public void stopMotors(int motor){
 
 
     public boolean isMotorExtended(int motor){//is the motor(s) extended?
-      if (motor<1 && getMotorPos[0].apply() < EncoderPos.extendLeft){
+      if (motor<1 && leftEncoder.getPosition() < (double) dEncoderPos.get(EncoderPos.extendLeft)){
         return false;
       }
-      if (motor>-1 && getMotorPos[1].apply() < EncoderPos.extendRight){
+      if (motor>-1 && rightEncoder.getPosition() < (double) dEncoderPos.get(EncoderPos.extendRight)){
         return false;
       }
       return true;
     }
 
     public boolean isMotorRetracted(int motor){//is the motor(s) retracted?
-      if (motor<1 && getMotorPos[0].apply() > EncoderPos.retractLeft){
+      if (motor<1 && leftEncoder.getPosition() > (double) dEncoderPos.get(EncoderPos.retractLeft)){
         return false;
       }
-      if (motor>-1 && getMotorPos[1].apply() > EncoderPos.retractRight){
+      if (motor>-1 && rightEncoder.getPosition() > (double) dEncoderPos.get(EncoderPos.retractRight)){
         return false;
       }
       return true;

From b598f631f4abdbd6c982689c398344f51159e17d Mon Sep 17 00:00:00 2001
From: FriedLongJohns <81837862+FriedLongJohns@users.noreply.github.com>
Date: Sun, 18 Sep 2022 21:13:42 -0700
Subject: [PATCH 09/28] keep the SmartDashboard keys/values consistent to the
 rest of the code

Co-authored-by: Kedas <m@yarn.network>
---
 src/main/java/org/team199/robot2022/subsystems/Climber.java | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/org/team199/robot2022/subsystems/Climber.java b/src/main/java/org/team199/robot2022/subsystems/Climber.java
index 157006b..86182f1 100644
--- a/src/main/java/org/team199/robot2022/subsystems/Climber.java
+++ b/src/main/java/org/team199/robot2022/subsystems/Climber.java
@@ -111,7 +111,7 @@ public void stopMotors(int motor){
       }
       if (motor>-1){
         right.set(0);
-        SmartDashboard.putString("Right climber is", "Stopped");
+        SmartDashboard.putString("Right Climber State", "Stop");
       }
     }
 

From 95bb19611183525b147cb316621ff6bce444b82a Mon Sep 17 00:00:00 2001
From: FriedLongJohns <81837862+FriedLongJohns@users.noreply.github.com>
Date: Sun, 18 Sep 2022 21:14:04 -0700
Subject: [PATCH 10/28] make more readable

Co-authored-by: Kedas <m@yarn.network>
---
 .../team199/robot2022/subsystems/Climber.java | 24 +++++++++++++++----
 1 file changed, 19 insertions(+), 5 deletions(-)

diff --git a/src/main/java/org/team199/robot2022/subsystems/Climber.java b/src/main/java/org/team199/robot2022/subsystems/Climber.java
index 86182f1..645b74c 100644
--- a/src/main/java/org/team199/robot2022/subsystems/Climber.java
+++ b/src/main/java/org/team199/robot2022/subsystems/Climber.java
@@ -52,16 +52,30 @@ public static enum EncoderPos{extendLeft,extendRight,retractLeft,retractRight,ze
     private final RelativeEncoder rightEncoder = right.getEncoder();
 
 
-    private final Consumer[] setEncoder = new Consumer[]{//faster to array[](inp) than a bunch of if's in a func
+    private final Consumer[] setEncoder = new Consumer[]{ // faster to array[](inp) than a bunch of if's in a func
       (pos) -> leftEncoder.setPosition((double) pos),
-      (pos) -> {leftEncoder.setPosition((double) pos);rightEncoder.setPosition((double) pos);},
+      (pos) -> {
+        leftEncoder.setPosition((double) pos);
+        rightEncoder.setPosition((double) pos);
+      },
       (pos) -> rightEncoder.setPosition((double) pos),
     };
 
     private final Consumer[] setMotor = new Consumer[]{
-      (speed) -> {left.set((double) speed);SmartDashboard.putString("Left Climber State", "Moving");},
-      (speed) -> {left.set((double) speed);right.set((double) speed);SmartDashboard.putString("Left Climber State", "Moving");SmartDashboard.putString("Right Climber State", "Moving");},
-      (speed) -> {right.set((double) speed);SmartDashboard.putString("Right Climber State", "Moving");},
+      (speed) -> {
+        left.set((double) speed);
+        SmartDashboard.putString("Left Climber State", "Moving");
+      },
+      (speed) -> {
+        left.set((double) speed);
+        right.set((double) speed);
+        SmartDashboard.putString("Left Climber State", "Moving");
+        SmartDashboard.putString("Right Climber State", "Moving");
+      },
+      (speed) -> {
+        right.set((double) speed);
+        SmartDashboard.putString("Right Climber State", "Moving");
+      },
     };
 
     public Climber() {

From 77ea14dfb85fb20fff92540c802f51f78697e8b6 Mon Sep 17 00:00:00 2001
From: FriedLongJohns <81837862+FriedLongJohns@users.noreply.github.com>
Date: Sun, 18 Sep 2022 21:14:17 -0700
Subject: [PATCH 11/28] keep the SmartDashboard keys/values consistent to the
 rest of the code

Co-authored-by: Kedas <m@yarn.network>
---
 src/main/java/org/team199/robot2022/subsystems/Climber.java | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/org/team199/robot2022/subsystems/Climber.java b/src/main/java/org/team199/robot2022/subsystems/Climber.java
index 645b74c..9321fa3 100644
--- a/src/main/java/org/team199/robot2022/subsystems/Climber.java
+++ b/src/main/java/org/team199/robot2022/subsystems/Climber.java
@@ -121,7 +121,7 @@ public void moveMotors(MotorSpeed speed, int motor){
     public void stopMotors(int motor){
       if (motor<1){
         left.set(0);
-        SmartDashboard.putString("Left climber is", "Stopped");
+        SmartDashboard.putString("Left Climber State", "Stop");
       }
       if (motor>-1){
         right.set(0);

From eb54633f9ee96406c6a23c13db31f1e68191ee23 Mon Sep 17 00:00:00 2001
From: FriedLongJohns <81837862+FriedLongJohns@users.noreply.github.com>
Date: Wed, 21 Sep 2022 17:25:00 -0700
Subject: [PATCH 12/28] Call static things more statically

mmm yes what a good push title
---
 src/main/java/org/team199/robot2022/RobotContainer.java | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/src/main/java/org/team199/robot2022/RobotContainer.java b/src/main/java/org/team199/robot2022/RobotContainer.java
index 5b64ece..5b9ad05 100644
--- a/src/main/java/org/team199/robot2022/RobotContainer.java
+++ b/src/main/java/org/team199/robot2022/RobotContainer.java
@@ -128,10 +128,10 @@ private void configureButtonBindingsLeftJoy() {
 
   private void configureButtonBindingsRightJoy() {
     new JoystickButton(rightJoy, Constants.OI.RightJoy.shootPort).whenPressed(new Shoot(intakeFeeder, shooter));
-    new JoystickButton(rightJoy, Constants.OI.RightJoy.slowExtendLeftClimberPort).whileHeld(new InstantCommand(()->climber.moveMotors(Climber.MotorSpeed.slowExtend,climber.leftMotor))).whenReleased(new InstantCommand(()->climber.stopMotors(climber.leftMotor)));
-    new JoystickButton(rightJoy, Constants.OI.RightJoy.slowRetractLeftClimberPort).whileHeld(new InstantCommand(()->climber.moveMotors(Climber.MotorSpeed.slowRetract,climber.leftMotor))).whenReleased(new InstantCommand(()->climber.stopMotors(climber.leftMotor)));
-    new JoystickButton(rightJoy, Constants.OI.RightJoy.slowExtendRightClimberPort).whileHeld(new InstantCommand(()->climber.moveMotors(Climber.MotorSpeed.slowExtend,climber.rightMotor))).whenReleased(new InstantCommand(()->climber.stopMotors(climber.rightMotor)));
-    new JoystickButton(rightJoy, Constants.OI.RightJoy.slowRetractRightClimberPort).whileHeld(new InstantCommand(()->climber.moveMotors(Climber.MotorSpeed.slowRetract,climber.rightMotor))).whenReleased(new InstantCommand(()->climber.stopMotors(climber.rightMotor)));
+    new JoystickButton(rightJoy, Constants.OI.RightJoy.slowExtendLeftClimberPort).whileHeld(new InstantCommand(()->climber.moveMotors(Climber.MotorSpeed.slowExtend,Climber.leftMotor))).whenReleased(new InstantCommand(()->climber.stopMotors(Climber.leftMotor)));
+    new JoystickButton(rightJoy, Constants.OI.RightJoy.slowRetractLeftClimberPort).whileHeld(new InstantCommand(()->climber.moveMotors(Climber.MotorSpeed.slowRetract,Climber.leftMotor))).whenReleased(new InstantCommand(()->climber.stopMotors(Climber.leftMotor)));
+    new JoystickButton(rightJoy, Constants.OI.RightJoy.slowExtendRightClimberPort).whileHeld(new InstantCommand(()->climber.moveMotors(Climber.MotorSpeed.slowExtend,Climber.rightMotor))).whenReleased(new InstantCommand(()->climber.stopMotors(Climber.rightMotor)));
+    new JoystickButton(rightJoy, Constants.OI.RightJoy.slowRetractRightClimberPort).whileHeld(new InstantCommand(()->climber.moveMotors(Climber.MotorSpeed.slowRetract,Climber.rightMotor))).whenReleased(new InstantCommand(()->climber.stopMotors(Climber.rightMotor)));
   }
 
   private void configureButtonBindingsController() {

From 6f254dd0eda0171b331109c805fec2290f57e62b Mon Sep 17 00:00:00 2001
From: FriedLongJohns <81837862+FriedLongJohns@users.noreply.github.com>
Date: Wed, 21 Sep 2022 17:25:40 -0700
Subject: [PATCH 13/28] Go back to using ParallelCommandGroup

Reject Humanity, return to Monke
---
 .../robot2022/commands/ExtendClimber.java     | 35 ++++++++++---------
 .../robot2022/commands/RetractClimber.java    | 35 ++++++++++---------
 2 files changed, 36 insertions(+), 34 deletions(-)

diff --git a/src/main/java/org/team199/robot2022/commands/ExtendClimber.java b/src/main/java/org/team199/robot2022/commands/ExtendClimber.java
index b34aeaf..0249dc8 100644
--- a/src/main/java/org/team199/robot2022/commands/ExtendClimber.java
+++ b/src/main/java/org/team199/robot2022/commands/ExtendClimber.java
@@ -6,25 +6,26 @@
 
 import org.team199.robot2022.subsystems.Climber;
 
-import edu.wpi.first.wpilibj2.command.CommandBase;
+import edu.wpi.first.wpilibj2.command.FunctionalCommand;
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 
-public class ExtendClimber extends CommandBase {
-    private Climber climber;
+public class ExtendClimber extends ParallelCommandGroup {
 
     public ExtendClimber(Climber climber) {
-        addRequirements(this.climber = climber);
+        super(
+            new FunctionalCommand(
+                () -> {},
+                () -> {climber.moveMotors(Climber.MotorSpeed.extend,climber.rightMotor);},
+                (interrupted) -> {climber.stopMotors(climber.rightMotor);},
+                () -> {return climber.isMotorExtended(climber.rightMotor);}
+            ),
+            new FunctionalCommand(
+                () -> {},
+                () -> {climber.moveMotors(Climber.MotorSpeed.extend,climber.leftMotor);},
+                (interrupted) -> {climber.stopMotors(climber.leftMotor);},
+                () -> {return climber.isMotorExtended(climber.leftMotor);}
+            )
+        );
+        addRequirements(climber);
     }
-
-    public void initialize() {
-        climber.moveMotors(Climber.MotorSpeed.extend,climber.bothMotors);
-    }
-
-    public boolean isFinished(){
-        return climber.isMotorExtended(climber.bothMotors);
-    }
-
-    public void end(){
-        climber.stopMotors(climber.bothMotors);
-    }
-
 }
diff --git a/src/main/java/org/team199/robot2022/commands/RetractClimber.java b/src/main/java/org/team199/robot2022/commands/RetractClimber.java
index 9e4ade6..a5dadcc 100644
--- a/src/main/java/org/team199/robot2022/commands/RetractClimber.java
+++ b/src/main/java/org/team199/robot2022/commands/RetractClimber.java
@@ -6,25 +6,26 @@
 
 import org.team199.robot2022.subsystems.Climber;
 
-import edu.wpi.first.wpilibj2.command.CommandBase;
+import edu.wpi.first.wpilibj2.command.FunctionalCommand;
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 
-public class RetractClimber extends CommandBase {
-    private Climber climber;
+public class RetractClimber extends ParallelCommandGroup {
 
     public RetractClimber(Climber climber) {
-        addRequirements(this.climber = climber);
+        super(
+            new FunctionalCommand(
+              () -> {},
+              () -> {climber.moveMotors(Climber.MotorSpeed.retract,climber.rightMotor);},
+              (interrupted) -> {climber.stopMotors(climber.rightMotor);},
+              () -> {return climber.isMotorRetracted(climber.rightMotor);}
+            ),
+            new FunctionalCommand(
+              () -> {},
+              () -> {climber.moveMotors(Climber.MotorSpeed.retract,climber.leftMotor);},
+              (interrupted) -> {climber.stopMotors(climber.leftMotor);},
+              () -> {return climber.isMotorRetracted(climber.leftMotor);}
+            )
+        );
+        addRequirements(climber);
     }
-
-    public void initialize() {
-        climber.moveMotors(Climber.MotorSpeed.retract,climber.bothMotors);
-    }
-
-    public boolean isFinished(){
-        return climber.isMotorRetracted(climber.bothMotors);
-    }
-
-    public void end(){
-        climber.stopMotors(climber.bothMotors);
-    }
-
 }

From c34f37129c4dfa4af0afb244612b7c6ad7fcd368 Mon Sep 17 00:00:00 2001
From: FriedLongJohns <81837862+FriedLongJohns@users.noreply.github.com>
Date: Wed, 21 Sep 2022 17:27:00 -0700
Subject: [PATCH 14/28] "assign a member variable to these enums instead of
 using a dictionary"

---
 .../team199/robot2022/subsystems/Climber.java | 58 +++++++++++--------
 1 file changed, 34 insertions(+), 24 deletions(-)

diff --git a/src/main/java/org/team199/robot2022/subsystems/Climber.java b/src/main/java/org/team199/robot2022/subsystems/Climber.java
index 9321fa3..97b3bdd 100644
--- a/src/main/java/org/team199/robot2022/subsystems/Climber.java
+++ b/src/main/java/org/team199/robot2022/subsystems/Climber.java
@@ -33,12 +33,33 @@ public class Climber extends SubsystemBase {
     private static final double kVoltsToCounterTorque = 10.5;
     private static final double kSlowVoltsToCounterTorque = (1.1D / 32) * 12;
 
-    public static enum MotorSpeed{retract,extend,slowRetract,slowExtend;};
-    private static Dictionary dMotorSpeed = new Hashtable();//given values in constructor
+    public static enum MotorSpeed{
+      retract(-((kDesiredRetractSpeedInps / kInPerSec) + (kVoltsToCounterTorque / 12))),// ~ -0.06151
+      extend(kDesiredExtendSpeedInps / kInPerSec),// ~0.30261
+      slowRetract(-((kSlowDesiredRetractSpeedInps / kInPerSec) + (kSlowVoltsToCounterTorque / 12))),// ~ -0.06151
+      slowExtend(kSlowDesiredExtendSpeedInps / kInPerSec);// ~0.30261
 
+      public final double value;
+
+      private MotorSpeed(double value) {
+          this.value = value;
+      }
+    };
 
-    public static enum EncoderPos{extendLeft,extendRight,retractLeft,retractRight,zero;};
-    private static Dictionary dEncoderPos = new Hashtable();//given values in constructor
+
+    public static enum EncoderPos{
+      extendLeft(-5.317),
+      extendRight(5.315),
+      retractLeft(-1.151),
+      retractRight(-1.172),
+      zero(0.0);
+
+      public final double value;
+
+      private EncoderPos(double value) {
+          this.value = value;
+      }
+    };
 
 
     public static final int leftMotor = -1;//because someone requested this and there's no point slapping them in an enum
@@ -90,17 +111,6 @@ public Climber() {
         SmartDashboard.putString("Right Climber State", "Stop");
         SmartDashboard.putNumber("kDesiredExtendSpeedInps", kDesiredExtendSpeedInps);
         SmartDashboard.putNumber("kDesiredRetractSpeedInps", kDesiredRetractSpeedInps);
-
-        dEncoderPos.put(EncoderPos.extendRight, 5.315);
-        dEncoderPos.put(EncoderPos.retractLeft, -1.151);
-        dEncoderPos.put(EncoderPos.extendLeft, -5.317);
-        dEncoderPos.put(EncoderPos.retractRight, -1.172);
-        dEncoderPos.put(EncoderPos.zero, 0.0);
-
-        dMotorSpeed.put(MotorSpeed.retract, -((kDesiredRetractSpeedInps / kInPerSec) + (kVoltsToCounterTorque / 12))); // ~ -0.06151
-        dMotorSpeed.put(MotorSpeed.extend, (kDesiredExtendSpeedInps / kInPerSec)); // ~0.30261
-        dMotorSpeed.put(MotorSpeed.slowRetract, -((kSlowDesiredRetractSpeedInps / kInPerSec) + (kSlowVoltsToCounterTorque / 12))); // ~ -0.06151
-        dMotorSpeed.put(MotorSpeed.slowExtend, (kSlowDesiredExtendSpeedInps / kInPerSec)); // ~0.30261
     }
 
     @Override
@@ -110,12 +120,13 @@ public void periodic() {
     }
 
     //motor = -1 left or 1 right or 0 both
-    public void resetEncodersTo(EncoderPos pos,int motor){
-			setEncoder[motor+1].accept(dEncoderPos.get(pos));
+    public void resetEncodersTo(EncoderPos posEnum,int motor){
+			// setEncoder[motor+1].accept(dEncoderPos.get(pos));
+      setEncoder[motor+1].accept(posEnum.value);
     }
 
-    public void moveMotors(MotorSpeed speed, int motor){
-      setMotor[motor+1].accept(dMotorSpeed.get(speed));
+    public void moveMotors(MotorSpeed speedEnum, int motor){
+      setMotor[motor+1].accept(speedEnum.value);
     }
 
     public void stopMotors(int motor){
@@ -129,22 +140,21 @@ public void stopMotors(int motor){
       }
     }
 
-
     public boolean isMotorExtended(int motor){//is the motor(s) extended?
-      if (motor<1 && leftEncoder.getPosition() < (double) dEncoderPos.get(EncoderPos.extendLeft)){
+      if (motor<1 && leftEncoder.getPosition() < EncoderPos.extendLeft.value){
         return false;
       }
-      if (motor>-1 && rightEncoder.getPosition() < (double) dEncoderPos.get(EncoderPos.extendRight)){
+      if (motor>-1 && rightEncoder.getPosition() < EncoderPos.extendRight.value){
         return false;
       }
       return true;
     }
 
     public boolean isMotorRetracted(int motor){//is the motor(s) retracted?
-      if (motor<1 && leftEncoder.getPosition() > (double) dEncoderPos.get(EncoderPos.retractLeft)){
+      if (motor<1 && leftEncoder.getPosition() > EncoderPos.retractLeft.value){
         return false;
       }
-      if (motor>-1 && rightEncoder.getPosition() > (double) dEncoderPos.get(EncoderPos.retractRight)){
+      if (motor>-1 && rightEncoder.getPosition() > EncoderPos.retractRight.value){
         return false;
       }
       return true;

From 6b6c82c7a3fb7c149c162870e41e0b644fb3519d Mon Sep 17 00:00:00 2001
From: FriedLongJohns <81837862+FriedLongJohns@users.noreply.github.com>
Date: Wed, 21 Sep 2022 17:28:44 -0700
Subject: [PATCH 15/28] Make commands "final"

---
 src/main/java/org/team199/robot2022/commands/ExtendClimber.java | 2 +-
 .../java/org/team199/robot2022/commands/RetractClimber.java     | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/java/org/team199/robot2022/commands/ExtendClimber.java b/src/main/java/org/team199/robot2022/commands/ExtendClimber.java
index 0249dc8..3ce75b4 100644
--- a/src/main/java/org/team199/robot2022/commands/ExtendClimber.java
+++ b/src/main/java/org/team199/robot2022/commands/ExtendClimber.java
@@ -9,7 +9,7 @@
 import edu.wpi.first.wpilibj2.command.FunctionalCommand;
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 
-public class ExtendClimber extends ParallelCommandGroup {
+public final class ExtendClimber extends ParallelCommandGroup {
 
     public ExtendClimber(Climber climber) {
         super(
diff --git a/src/main/java/org/team199/robot2022/commands/RetractClimber.java b/src/main/java/org/team199/robot2022/commands/RetractClimber.java
index a5dadcc..a6c8413 100644
--- a/src/main/java/org/team199/robot2022/commands/RetractClimber.java
+++ b/src/main/java/org/team199/robot2022/commands/RetractClimber.java
@@ -9,7 +9,7 @@
 import edu.wpi.first.wpilibj2.command.FunctionalCommand;
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 
-public class RetractClimber extends ParallelCommandGroup {
+public final class RetractClimber extends ParallelCommandGroup {
 
     public RetractClimber(Climber climber) {
         super(

From 3e73f7257e9d9aa522e4fd02916fb3fcd62081d7 Mon Sep 17 00:00:00 2001
From: FriedLongJohns <81837862+FriedLongJohns@users.noreply.github.com>
Date: Wed, 21 Sep 2022 17:30:54 -0700
Subject: [PATCH 16/28] Use DoubleConsumer instead of Consumer + Cast

---
 .../team199/robot2022/subsystems/Climber.java | 22 +++++++++----------
 1 file changed, 11 insertions(+), 11 deletions(-)

diff --git a/src/main/java/org/team199/robot2022/subsystems/Climber.java b/src/main/java/org/team199/robot2022/subsystems/Climber.java
index 97b3bdd..936776b 100644
--- a/src/main/java/org/team199/robot2022/subsystems/Climber.java
+++ b/src/main/java/org/team199/robot2022/subsystems/Climber.java
@@ -9,7 +9,7 @@
 import frc.robot.lib.MotorControllerFactory;
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.RelativeEncoder;
-import java.util.function.Consumer;
+import java.util.function.DoubleConsumer;
 import java.util.Hashtable;
 import java.util.Dictionary;
 
@@ -73,28 +73,28 @@ private EncoderPos(double value) {
     private final RelativeEncoder rightEncoder = right.getEncoder();
 
 
-    private final Consumer[] setEncoder = new Consumer[]{ // faster to array[](inp) than a bunch of if's in a func
-      (pos) -> leftEncoder.setPosition((double) pos),
+    private final DoubleConsumer[] setEncoder = new DoubleConsumer[]{ // faster to array[](inp) than a bunch of if's in a func
+      (pos) -> leftEncoder.setPosition(pos),
       (pos) -> {
-        leftEncoder.setPosition((double) pos);
-        rightEncoder.setPosition((double) pos);
+        leftEncoder.setPosition(pos);
+        rightEncoder.setPosition(pos);
       },
-      (pos) -> rightEncoder.setPosition((double) pos),
+      (pos) -> rightEncoder.setPosition(pos),
     };
 
-    private final Consumer[] setMotor = new Consumer[]{
+    private final DoubleConsumer[] setMotor = new DoubleConsumer[]{
       (speed) -> {
-        left.set((double) speed);
+        left.set(speed);
         SmartDashboard.putString("Left Climber State", "Moving");
       },
       (speed) -> {
-        left.set((double) speed);
-        right.set((double) speed);
+        left.set(speed);
+        right.set(speed);
         SmartDashboard.putString("Left Climber State", "Moving");
         SmartDashboard.putString("Right Climber State", "Moving");
       },
       (speed) -> {
-        right.set((double) speed);
+        right.set(speed);
         SmartDashboard.putString("Right Climber State", "Moving");
       },
     };

From 7fe7869212bc7bc94d693a9f55df6226c7b386be Mon Sep 17 00:00:00 2001
From: FriedLongJohns <81837862+FriedLongJohns@users.noreply.github.com>
Date: Wed, 21 Sep 2022 17:32:01 -0700
Subject: [PATCH 17/28] Stop -> Stopped

---
 .../java/org/team199/robot2022/subsystems/Climber.java    | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/src/main/java/org/team199/robot2022/subsystems/Climber.java b/src/main/java/org/team199/robot2022/subsystems/Climber.java
index 936776b..6984459 100644
--- a/src/main/java/org/team199/robot2022/subsystems/Climber.java
+++ b/src/main/java/org/team199/robot2022/subsystems/Climber.java
@@ -107,8 +107,8 @@ public Climber() {
         leftEncoder.setPosition(0);
         rightEncoder.setPositionConversionFactor(1 / gearing);
         rightEncoder.setPosition(0);
-        SmartDashboard.putString("Left Climber State", "Stop");
-        SmartDashboard.putString("Right Climber State", "Stop");
+        SmartDashboard.putString("Left Climber State", "Stopped");
+        SmartDashboard.putString("Right Climber State", "Stopped");
         SmartDashboard.putNumber("kDesiredExtendSpeedInps", kDesiredExtendSpeedInps);
         SmartDashboard.putNumber("kDesiredRetractSpeedInps", kDesiredRetractSpeedInps);
     }
@@ -132,11 +132,11 @@ public void moveMotors(MotorSpeed speedEnum, int motor){
     public void stopMotors(int motor){
       if (motor<1){
         left.set(0);
-        SmartDashboard.putString("Left Climber State", "Stop");
+        SmartDashboard.putString("Left Climber State", "Stopped");
       }
       if (motor>-1){
         right.set(0);
-        SmartDashboard.putString("Right Climber State", "Stop");
+        SmartDashboard.putString("Right Climber State", "Stopped");
       }
     }
 

From 5d3148ba2cc52907d9dc8424fc2c5ce958a413f9 Mon Sep 17 00:00:00 2001
From: FriedLongJohns <81837862+FriedLongJohns@users.noreply.github.com>
Date: Fri, 23 Sep 2022 20:26:56 -0700
Subject: [PATCH 18/28] Fix Conflicts

---
 .../java/org/team199/robot2022/RobotContainer.java    | 11 +++++++++++
 1 file changed, 11 insertions(+)

diff --git a/src/main/java/org/team199/robot2022/RobotContainer.java b/src/main/java/org/team199/robot2022/RobotContainer.java
index 5b9ad05..98ca18b 100644
--- a/src/main/java/org/team199/robot2022/RobotContainer.java
+++ b/src/main/java/org/team199/robot2022/RobotContainer.java
@@ -24,6 +24,8 @@
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj.PowerDistribution;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.ConditionalCommand;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
@@ -58,7 +60,9 @@ public class RobotContainer {
 
   public final DigitalInput[] autoSelectors;
   public final AutoPath[] autoPaths;
+  private final SendableChooser<AutoPath> autoSelector = new SendableChooser<>();
 
+  private final boolean inCompetition = true;
 
   /**
    * The container for the robot. Contains subsystems, OI devices, and commands.
@@ -124,6 +128,11 @@ private void configureButtonBindingsLeftJoy() {
     new JoystickButton(leftJoy, Constants.OI.LeftJoy.manualAddPort).whenPressed(new InstantCommand(intakeFeeder::manualAdd));
     new JoystickButton(leftJoy, Constants.OI.LeftJoy.manualSubtractPort).whenPressed(new InstantCommand(intakeFeeder::manualSub));
     new JoystickButton(leftJoy, Constants.OI.LeftJoy.overridePort).whenPressed(new InstantCommand(intakeFeeder::override));
+
+    new JoystickButton(leftJoy, Constants.OI.LeftJoy.resetClimberEncoders). whenPressed(new InstantCommand(()->climber.resetEncodersTo(Climber.EncoderPos.zero,Climber.bothMotors)));
+    new JoystickButton(leftJoy, Constants.OI.LeftJoy.toggleDriveMode).whenPressed(new InstantCommand( () -> {SmartDashboard.putBoolean("Field Oriented", SmartDashboard.getBoolean("Field Oriented", true) ? false : true);}));
+    new JoystickButton(leftJoy, Constants.OI.LeftJoy.toggleLongShot).whenPressed(new InstantCommand(shooter::toggleLongShot));
+    new JoystickButton(leftJoy, Constants.OI.LeftJoy.resetFieldOriented).whenPressed(new SequentialCommandGroup(new InstantCommand(() -> {SmartDashboard.putBoolean("Field Oriented", true);}), new WaitCommand(0.05), new InstantCommand(() -> {SmartDashboard.putNumber("Field Offset from North (degrees)", SmartDashboard.getNumber("Field Offset from North (degrees)", 0) - dt.getHeadingDeg() + 180);})));
   }
 
   private void configureButtonBindingsRightJoy() {
@@ -132,6 +141,8 @@ private void configureButtonBindingsRightJoy() {
     new JoystickButton(rightJoy, Constants.OI.RightJoy.slowRetractLeftClimberPort).whileHeld(new InstantCommand(()->climber.moveMotors(Climber.MotorSpeed.slowRetract,Climber.leftMotor))).whenReleased(new InstantCommand(()->climber.stopMotors(Climber.leftMotor)));
     new JoystickButton(rightJoy, Constants.OI.RightJoy.slowExtendRightClimberPort).whileHeld(new InstantCommand(()->climber.moveMotors(Climber.MotorSpeed.slowExtend,Climber.rightMotor))).whenReleased(new InstantCommand(()->climber.stopMotors(Climber.rightMotor)));
     new JoystickButton(rightJoy, Constants.OI.RightJoy.slowRetractRightClimberPort).whileHeld(new InstantCommand(()->climber.moveMotors(Climber.MotorSpeed.slowRetract,Climber.rightMotor))).whenReleased(new InstantCommand(()->climber.stopMotors(Climber.rightMotor)));
+
+    new JoystickButton(rightJoy, Constants.OI.RightJoy.overridePort).whenPressed(new InstantCommand(intakeFeeder::override));
   }
 
   private void configureButtonBindingsController() {

From 208ebf0813a70842f4113d63bc148546b749f9a9 Mon Sep 17 00:00:00 2001
From: FriedLongJohns <81837862+FriedLongJohns@users.noreply.github.com>
Date: Fri, 23 Sep 2022 20:29:28 -0700
Subject: [PATCH 19/28] Move enums to EOF + fix conflicts (probably)

---
 .../team199/robot2022/subsystems/Climber.java | 78 ++++++++++++-------
 1 file changed, 50 insertions(+), 28 deletions(-)

diff --git a/src/main/java/org/team199/robot2022/subsystems/Climber.java b/src/main/java/org/team199/robot2022/subsystems/Climber.java
index 6984459..aa0ac25 100644
--- a/src/main/java/org/team199/robot2022/subsystems/Climber.java
+++ b/src/main/java/org/team199/robot2022/subsystems/Climber.java
@@ -33,34 +33,8 @@ public class Climber extends SubsystemBase {
     private static final double kVoltsToCounterTorque = 10.5;
     private static final double kSlowVoltsToCounterTorque = (1.1D / 32) * 12;
 
-    public static enum MotorSpeed{
-      retract(-((kDesiredRetractSpeedInps / kInPerSec) + (kVoltsToCounterTorque / 12))),// ~ -0.06151
-      extend(kDesiredExtendSpeedInps / kInPerSec),// ~0.30261
-      slowRetract(-((kSlowDesiredRetractSpeedInps / kInPerSec) + (kSlowVoltsToCounterTorque / 12))),// ~ -0.06151
-      slowExtend(kSlowDesiredExtendSpeedInps / kInPerSec);// ~0.30261
-
-      public final double value;
-
-      private MotorSpeed(double value) {
-          this.value = value;
-      }
-    };
-
-
-    public static enum EncoderPos{
-      extendLeft(-5.317),
-      extendRight(5.315),
-      retractLeft(-1.151),
-      retractRight(-1.172),
-      zero(0.0);
-
-      public final double value;
-
-      private EncoderPos(double value) {
-          this.value = value;
-      }
-    };
-
+    private boolean keepPosition = true;
+    private double holdTolerance = 0.05;
 
     public static final int leftMotor = -1;//because someone requested this and there's no point slapping them in an enum
     public static final int bothMotors = 0;
@@ -117,6 +91,26 @@ public Climber() {
     public void periodic() {
         SmartDashboard.putNumber("L Climber Pos", leftEncoder.getPosition());
         SmartDashboard.putNumber("R Climber Pos", rightEncoder.getPosition());
+
+        holdTolerance = SmartDashboard.getNumber("Climber: Tolerance", holdTolerance);
+        SmartDashboard.putNumber("Climber: Tolerance", holdTolerance);
+        SmartDashboard.putBoolean("Climber: Keep Zeroed", keepPosition);
+        keepZeroed();
+    }
+
+    public void keepZeroed() {
+        if(keepPosition) {
+            if(Math.abs(getLeftPosition()) > holdTolerance) {
+                left.set(Math.signum(getLeftPosition()) > 0 ? kSlowRetractSpeed : kSlowExtendSpeed);
+            } else {
+                left.set(0);
+            }
+            if(Math.abs(getRightPosition()) > holdTolerance) {
+                right.set(Math.signum(getRightPosition()) > 0 ? kSlowRetractSpeed : kSlowExtendSpeed);
+            } else {
+                right.set(0);
+            }
+        }
     }
 
     //motor = -1 left or 1 right or 0 both
@@ -160,4 +154,32 @@ public boolean isMotorRetracted(int motor){//is the motor(s) retracted?
       return true;
     }
 
+    public static enum MotorSpeed{
+      retract(-((kDesiredRetractSpeedInps / kInPerSec) + (kVoltsToCounterTorque / 12))),// ~ -0.06151
+      extend(kDesiredExtendSpeedInps / kInPerSec),// ~0.30261
+      slowRetract(-((kSlowDesiredRetractSpeedInps / kInPerSec) + (kSlowVoltsToCounterTorque / 12))),// ~ -0.06151
+      slowExtend(kSlowDesiredExtendSpeedInps / kInPerSec);// ~0.30261
+
+      public final double value;
+
+      private MotorSpeed(double value) {
+          this.value = value;
+      }
+    };
+
+
+    public static enum EncoderPos{
+      extendLeft(-5.317),
+      extendRight(5.315),
+      retractLeft(-1.151),
+      retractRight(-1.172),
+      zero(0.0);
+
+      public final double value;
+
+      private EncoderPos(double value) {
+          this.value = value;
+      }
+    };
+
 }

From 197c6aa3c0e116ec031abb489a0a5c6b4dc592ea Mon Sep 17 00:00:00 2001
From: FriedLongJohns <81837862+FriedLongJohns@users.noreply.github.com>
Date: Fri, 23 Sep 2022 20:46:41 -0700
Subject: [PATCH 20/28] merge conflicts

---
 .../team199/robot2022/subsystems/Climber.java | 76 -------------------
 1 file changed, 76 deletions(-)

diff --git a/src/main/java/org/team199/robot2022/subsystems/Climber.java b/src/main/java/org/team199/robot2022/subsystems/Climber.java
index 9fa1bb2..bd1fc14 100644
--- a/src/main/java/org/team199/robot2022/subsystems/Climber.java
+++ b/src/main/java/org/team199/robot2022/subsystems/Climber.java
@@ -92,33 +92,8 @@ public Climber() {
 
     @Override
     public void periodic() {
-<<<<<<< HEAD
-        SmartDashboard.putNumber("Left Climber Position", getLeftPosition());
-        SmartDashboard.putNumber("Right Climber Position", getRightPosition());
-        holdTolerance = SmartDashboard.getNumber("Climber: Tolerance", holdTolerance);
-        SmartDashboard.putNumber("Climber: Tolerance", holdTolerance);
-        SmartDashboard.putBoolean("Climber: Keep Zeroed", keepPosition);
-        keepZeroed();
-    }
-
-    public void keepZeroed() {
-        if(keepPosition) {
-            if(Math.abs(getLeftPosition()) > holdTolerance) {
-                left.set(Math.signum(getLeftPosition()) > 0 ? kSlowRetractSpeed : kSlowExtendSpeed);
-            } else {
-                left.set(0);
-            }
-            if(Math.abs(getRightPosition()) > holdTolerance) {
-                right.set(Math.signum(getRightPosition()) > 0 ? kSlowRetractSpeed : kSlowExtendSpeed);
-            } else {
-                right.set(0);
-            }
-        }
-    }
-=======
         SmartDashboard.putNumber("L Climber Pos", leftEncoder.getPosition());
         SmartDashboard.putNumber("R Climber Pos", rightEncoder.getPosition());
->>>>>>> milkybeans
 
         holdTolerance = SmartDashboard.getNumber("Climber: Tolerance", holdTolerance);
         SmartDashboard.putNumber("Climber: Tolerance", holdTolerance);
@@ -141,56 +116,6 @@ public void keepZeroed() {
         }
     }
 
-<<<<<<< HEAD
-    public void extendLeft() {
-        left.set(kExtendSpeed);
-        SmartDashboard.putString("Left climber is", "Extending");
-        keepPosition = false;
-    }
-
-    public void extendRight() {
-        right.set(kExtendSpeed);
-        SmartDashboard.putString("Right climber is", "Extending");
-        keepPosition = false;
-    }
-
-    public void retractLeft() {
-        left.set(kRetractSpeed);
-        SmartDashboard.putString("Left climber is", "Retracting");
-        keepPosition = false;
-    }
-
-    public void retractRight() {
-        right.set(kRetractSpeed);
-        SmartDashboard.putString("Right climber is", "Retracting");
-        keepPosition = false;
-    }
-
-    public void slowExtendLeft() {
-        left.set(kSlowExtendSpeed);
-        SmartDashboard.putString("Left climber is", "Extending");
-        keepPosition = false;
-    }
-
-    public void slowExtendRight() {
-        right.set(kSlowExtendSpeed);
-        SmartDashboard.putString("Right climber is", "Extending");
-        keepPosition = false;
-    }
-    public void slowRetractLeft() {
-        left.set(kSlowRetractSpeed);
-        SmartDashboard.putString("Left climber is", "Retracting");
-        keepPosition = false;
-    }
-
-    public void slowRetractRight() {
-        right.set(kSlowRetractSpeed);
-        SmartDashboard.putString("Right climber is", "Retracting");
-        keepPosition = false;
-    }
-
-    public void stop() {
-=======
     //motor = -1 left or 1 right or 0 both
     public void resetEncodersTo(EncoderPos posEnum,int motor){
 			// setEncoder[motor+1].accept(dEncoderPos.get(pos));
@@ -203,7 +128,6 @@ public void moveMotors(MotorSpeed speedEnum, int motor){
 
     public void stopMotors(int motor){
       if (motor<1){
->>>>>>> milkybeans
         left.set(0);
         SmartDashboard.putString("Left Climber State", "Stopped");
       }

From 37f258acde16b199597162e2e615b77ef61ec316 Mon Sep 17 00:00:00 2001
From: FriedLongJohns <81837862+FriedLongJohns@users.noreply.github.com>
Date: Fri, 23 Sep 2022 20:59:49 -0700
Subject: [PATCH 21/28] fix conflicts

---
 src/main/java/org/team199/robot2022/Constants.java        | 1 +
 .../java/org/team199/robot2022/subsystems/Climber.java    | 8 ++++----
 2 files changed, 5 insertions(+), 4 deletions(-)

diff --git a/src/main/java/org/team199/robot2022/Constants.java b/src/main/java/org/team199/robot2022/Constants.java
index a489420..e2c07c8 100644
--- a/src/main/java/org/team199/robot2022/Constants.java
+++ b/src/main/java/org/team199/robot2022/Constants.java
@@ -151,6 +151,7 @@ public static final class LeftJoy {
 
             public static final int manualAddPort = 3;
             public static final int manualSubtractPort = 2;
+            public static final int overridePort = 5;
             public static final int shootSoftOnePort = 6;
             public static final int resetAndExtendClimberPort = 9;
             public static final int resetAndRetractClimberPort = 8;
diff --git a/src/main/java/org/team199/robot2022/subsystems/Climber.java b/src/main/java/org/team199/robot2022/subsystems/Climber.java
index bd1fc14..32b1798 100644
--- a/src/main/java/org/team199/robot2022/subsystems/Climber.java
+++ b/src/main/java/org/team199/robot2022/subsystems/Climber.java
@@ -103,13 +103,13 @@ public void periodic() {
 
     public void keepZeroed() {
         if(keepPosition) {
-            if(Math.abs(getLeftPosition()) > holdTolerance) {
-                left.set(Math.signum(getLeftPosition()) > 0 ? kSlowRetractSpeed : kSlowExtendSpeed);
+            if(Math.abs(leftEncoder.getPosition()) > holdTolerance) {
+                left.set(Math.signum(leftEncoder.getPosition()) > 0 ? MotorSpeed.slowRetract.value : MotorSpeed.slowExtend.value);
             } else {
                 left.set(0);
             }
-            if(Math.abs(getRightPosition()) > holdTolerance) {
-                right.set(Math.signum(getRightPosition()) > 0 ? kSlowRetractSpeed : kSlowExtendSpeed);
+            if(Math.abs(rightEncoder.getPosition()) > holdTolerance) {
+                right.set(Math.signum(rightEncoder.getPosition()) > 0 ? MotorSpeed.slowRetract.value : MotorSpeed.slowExtend.value);
             } else {
                 right.set(0);
             }

From 852cb3075315405b3346a8a55b2b36b0f934b208 Mon Sep 17 00:00:00 2001
From: CoolSpy3 <coolspythree@gmail.com>
Date: Wed, 28 Sep 2022 17:08:28 -0700
Subject: [PATCH 22/28] remove old code

---
 src/main/java/org/team199/robot2022/Constants.java      | 1 -
 src/main/java/org/team199/robot2022/RobotContainer.java | 1 -
 2 files changed, 2 deletions(-)

diff --git a/src/main/java/org/team199/robot2022/Constants.java b/src/main/java/org/team199/robot2022/Constants.java
index e2c07c8..a489420 100644
--- a/src/main/java/org/team199/robot2022/Constants.java
+++ b/src/main/java/org/team199/robot2022/Constants.java
@@ -151,7 +151,6 @@ public static final class LeftJoy {
 
             public static final int manualAddPort = 3;
             public static final int manualSubtractPort = 2;
-            public static final int overridePort = 5;
             public static final int shootSoftOnePort = 6;
             public static final int resetAndExtendClimberPort = 9;
             public static final int resetAndRetractClimberPort = 8;
diff --git a/src/main/java/org/team199/robot2022/RobotContainer.java b/src/main/java/org/team199/robot2022/RobotContainer.java
index 6d33761..5137ec7 100644
--- a/src/main/java/org/team199/robot2022/RobotContainer.java
+++ b/src/main/java/org/team199/robot2022/RobotContainer.java
@@ -147,7 +147,6 @@ public RobotContainer(Robot robot) {
   private void configureButtonBindingsLeftJoy() {
     new JoystickButton(leftJoy, Constants.OI.LeftJoy.manualAddPort).whenPressed(new InstantCommand(intakeFeeder::manualAdd));
     new JoystickButton(leftJoy, Constants.OI.LeftJoy.manualSubtractPort).whenPressed(new InstantCommand(intakeFeeder::manualSub));
-    new JoystickButton(leftJoy, Constants.OI.LeftJoy.overridePort).whenPressed(new InstantCommand(intakeFeeder::override));
 
     new JoystickButton(leftJoy, Constants.OI.LeftJoy.resetClimberEncoders). whenPressed(new InstantCommand(()->climber.resetEncodersTo(Climber.EncoderPos.zero,Climber.bothMotors)));
     new JoystickButton(leftJoy, Constants.OI.LeftJoy.toggleDriveMode).whenPressed(new InstantCommand( () -> {SmartDashboard.putBoolean("Field Oriented", SmartDashboard.getBoolean("Field Oriented", true) ? false : true);}));

From f64f14d2edb48ab63edf875b0083088dde2028c7 Mon Sep 17 00:00:00 2001
From: FriedLongJohns <81837862+FriedLongJohns@users.noreply.github.com>
Date: Thu, 29 Sep 2022 09:37:48 -0700
Subject: [PATCH 23/28] Simplify Lambdas + staticify

---
 .../team199/robot2022/commands/ExtendClimber.java    | 12 ++++++------
 .../team199/robot2022/commands/RetractClimber.java   | 12 ++++++------
 2 files changed, 12 insertions(+), 12 deletions(-)

diff --git a/src/main/java/org/team199/robot2022/commands/ExtendClimber.java b/src/main/java/org/team199/robot2022/commands/ExtendClimber.java
index 3ce75b4..712e7b1 100644
--- a/src/main/java/org/team199/robot2022/commands/ExtendClimber.java
+++ b/src/main/java/org/team199/robot2022/commands/ExtendClimber.java
@@ -15,15 +15,15 @@ public ExtendClimber(Climber climber) {
         super(
             new FunctionalCommand(
                 () -> {},
-                () -> {climber.moveMotors(Climber.MotorSpeed.extend,climber.rightMotor);},
-                (interrupted) -> {climber.stopMotors(climber.rightMotor);},
-                () -> {return climber.isMotorExtended(climber.rightMotor);}
+                () -> climber.moveMotors(Climber.MotorSpeed.extend,Climber.rightMotor),
+                (interrupted) -> climber.stopMotors(Climber.rightMotor),
+                () -> climber.isMotorExtended(Climber.rightMotor)
             ),
             new FunctionalCommand(
                 () -> {},
-                () -> {climber.moveMotors(Climber.MotorSpeed.extend,climber.leftMotor);},
-                (interrupted) -> {climber.stopMotors(climber.leftMotor);},
-                () -> {return climber.isMotorExtended(climber.leftMotor);}
+                () -> climber.moveMotors(Climber.MotorSpeed.extend,Climber.leftMotor),
+                (interrupted) -> climber.stopMotors(Climber.leftMotor),
+                () -> climber.isMotorExtended(Climber.leftMotor)
             )
         );
         addRequirements(climber);
diff --git a/src/main/java/org/team199/robot2022/commands/RetractClimber.java b/src/main/java/org/team199/robot2022/commands/RetractClimber.java
index a6c8413..0d27ffc 100644
--- a/src/main/java/org/team199/robot2022/commands/RetractClimber.java
+++ b/src/main/java/org/team199/robot2022/commands/RetractClimber.java
@@ -15,15 +15,15 @@ public RetractClimber(Climber climber) {
         super(
             new FunctionalCommand(
               () -> {},
-              () -> {climber.moveMotors(Climber.MotorSpeed.retract,climber.rightMotor);},
-              (interrupted) -> {climber.stopMotors(climber.rightMotor);},
-              () -> {return climber.isMotorRetracted(climber.rightMotor);}
+              () -> climber.moveMotors(Climber.MotorSpeed.retract,Climber.rightMotor),
+              (interrupted) -> climber.stopMotors(Climber.rightMotor),
+              () -> climber.isMotorRetracted(Climber.rightMotor)
             ),
             new FunctionalCommand(
               () -> {},
-              () -> {climber.moveMotors(Climber.MotorSpeed.retract,climber.leftMotor);},
-              (interrupted) -> {climber.stopMotors(climber.leftMotor);},
-              () -> {return climber.isMotorRetracted(climber.leftMotor);}
+              () -> climber.moveMotors(Climber.MotorSpeed.retract,Climber.leftMotor),
+              (interrupted) -> climber.stopMotors(Climber.leftMotor),
+              () -> climber.isMotorRetracted(Climber.leftMotor)
             )
         );
         addRequirements(climber);

From a4ca2e749430c4a645b74c29335345d396c4489b Mon Sep 17 00:00:00 2001
From: FriedLongJohns <81837862+FriedLongJohns@users.noreply.github.com>
Date: Thu, 29 Sep 2022 09:38:16 -0700
Subject: [PATCH 24/28] Better 'if' statements

---
 src/main/java/org/team199/robot2022/subsystems/Climber.java | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/main/java/org/team199/robot2022/subsystems/Climber.java b/src/main/java/org/team199/robot2022/subsystems/Climber.java
index 32b1798..b7e299f 100644
--- a/src/main/java/org/team199/robot2022/subsystems/Climber.java
+++ b/src/main/java/org/team199/robot2022/subsystems/Climber.java
@@ -131,7 +131,7 @@ public void stopMotors(int motor){
         left.set(0);
         SmartDashboard.putString("Left Climber State", "Stopped");
       }
-      if (motor>-1){
+      if (motor!=leftMotor){
         right.set(0);
         SmartDashboard.putString("Right Climber State", "Stopped");
       }
@@ -141,7 +141,7 @@ public boolean isMotorExtended(int motor){//is the motor(s) extended?
       if (motor<1 && leftEncoder.getPosition() < EncoderPos.extendLeft.value){
         return false;
       }
-      if (motor>-1 && rightEncoder.getPosition() < EncoderPos.extendRight.value){
+      if (motor!=leftMotor && rightEncoder.getPosition() < EncoderPos.extendRight.value){
         return false;
       }
       return true;
@@ -151,7 +151,7 @@ public boolean isMotorRetracted(int motor){//is the motor(s) retracted?
       if (motor<1 && leftEncoder.getPosition() > EncoderPos.retractLeft.value){
         return false;
       }
-      if (motor>-1 && rightEncoder.getPosition() > EncoderPos.retractRight.value){
+      if (motor!=leftMotor && rightEncoder.getPosition() > EncoderPos.retractRight.value){
         return false;
       }
       return true;

From 832241958ce808ec78bc18be40c23e3466e71c07 Mon Sep 17 00:00:00 2001
From: FriedLongJohns <81837862+FriedLongJohns@users.noreply.github.com>
Date: Thu, 29 Sep 2022 09:40:00 -0700
Subject: [PATCH 25/28] change some constants

-change EncoderPos constants
-change kSlowDesired(Retract|Extend)SpeedInps
---
 .../org/team199/robot2022/subsystems/Climber.java    | 12 ++++++------
 1 file changed, 6 insertions(+), 6 deletions(-)

diff --git a/src/main/java/org/team199/robot2022/subsystems/Climber.java b/src/main/java/org/team199/robot2022/subsystems/Climber.java
index b7e299f..240cba6 100644
--- a/src/main/java/org/team199/robot2022/subsystems/Climber.java
+++ b/src/main/java/org/team199/robot2022/subsystems/Climber.java
@@ -27,8 +27,8 @@ public class Climber extends SubsystemBase {
 
     private static final double gearing = 9;
     private static final double kInPerSec = ((kNEOFreeSpeedRPM / gearing) * Math.PI * kDiameterIn / 60);
-    private static final double kSlowDesiredRetractSpeedInps = 1;
-    private static final double kSlowDesiredExtendSpeedInps = 1;
+    private static final double kSlowDesiredRetractSpeedInps = 2;
+    private static final double kSlowDesiredExtendSpeedInps = 2;
 
     // Torque is 2 * 9 * 0.5 = 9
     // Torque on the motor is Torque / ( gearing = 9 ) = 1
@@ -172,10 +172,10 @@ private MotorSpeed(double value) {
 
 
     public static enum EncoderPos{
-      extendLeft(-5.317),
-      extendRight(5.315),
-      retractLeft(-1.151),
-      retractRight(-1.172),
+      extendLeft(6.315),
+      extendRight(6.315),
+      retractLeft(-0.6),
+      retractRight(-0.6),
       zero(0.0);
 
       public final double value;

From e02564d1e59950130fb2e142ea6d9b000c170e42 Mon Sep 17 00:00:00 2001
From: FriedLongJohns <81837862+FriedLongJohns@users.noreply.github.com>
Date: Thu, 29 Sep 2022 09:43:44 -0700
Subject: [PATCH 26/28] Make things not final ...?

---
 src/main/java/org/team199/robot2022/commands/ExtendClimber.java | 2 +-
 .../java/org/team199/robot2022/commands/RetractClimber.java     | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/java/org/team199/robot2022/commands/ExtendClimber.java b/src/main/java/org/team199/robot2022/commands/ExtendClimber.java
index 712e7b1..1fcfc03 100644
--- a/src/main/java/org/team199/robot2022/commands/ExtendClimber.java
+++ b/src/main/java/org/team199/robot2022/commands/ExtendClimber.java
@@ -9,7 +9,7 @@
 import edu.wpi.first.wpilibj2.command.FunctionalCommand;
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 
-public final class ExtendClimber extends ParallelCommandGroup {
+public class ExtendClimber extends ParallelCommandGroup {
 
     public ExtendClimber(Climber climber) {
         super(
diff --git a/src/main/java/org/team199/robot2022/commands/RetractClimber.java b/src/main/java/org/team199/robot2022/commands/RetractClimber.java
index 0d27ffc..cc2823a 100644
--- a/src/main/java/org/team199/robot2022/commands/RetractClimber.java
+++ b/src/main/java/org/team199/robot2022/commands/RetractClimber.java
@@ -9,7 +9,7 @@
 import edu.wpi.first.wpilibj2.command.FunctionalCommand;
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 
-public final class RetractClimber extends ParallelCommandGroup {
+public class RetractClimber extends ParallelCommandGroup {
 
     public RetractClimber(Climber climber) {
         super(

From 20bff4ce3a7dde54529624bb9a8db6d09c672172 Mon Sep 17 00:00:00 2001
From: FriedLongJohns <81837862+FriedLongJohns@users.noreply.github.com>
Date: Sun, 2 Oct 2022 19:30:04 -0700
Subject: [PATCH 27/28] go make the rest of the IF statements better

---
 src/main/java/org/team199/robot2022/subsystems/Climber.java | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/main/java/org/team199/robot2022/subsystems/Climber.java b/src/main/java/org/team199/robot2022/subsystems/Climber.java
index 240cba6..bfbe614 100644
--- a/src/main/java/org/team199/robot2022/subsystems/Climber.java
+++ b/src/main/java/org/team199/robot2022/subsystems/Climber.java
@@ -127,7 +127,7 @@ public void moveMotors(MotorSpeed speedEnum, int motor){
     }
 
     public void stopMotors(int motor){
-      if (motor<1){
+      if (motor!=rightMotor){
         left.set(0);
         SmartDashboard.putString("Left Climber State", "Stopped");
       }
@@ -138,7 +138,7 @@ public void stopMotors(int motor){
     }
 
     public boolean isMotorExtended(int motor){//is the motor(s) extended?
-      if (motor<1 && leftEncoder.getPosition() < EncoderPos.extendLeft.value){
+      if (motor!=rightMotor && leftEncoder.getPosition() < EncoderPos.extendLeft.value){
         return false;
       }
       if (motor!=leftMotor && rightEncoder.getPosition() < EncoderPos.extendRight.value){
@@ -148,7 +148,7 @@ public boolean isMotorExtended(int motor){//is the motor(s) extended?
     }
 
     public boolean isMotorRetracted(int motor){//is the motor(s) retracted?
-      if (motor<1 && leftEncoder.getPosition() > EncoderPos.retractLeft.value){
+      if (motor!=rightMotor && leftEncoder.getPosition() > EncoderPos.retractLeft.value){
         return false;
       }
       if (motor!=leftMotor && rightEncoder.getPosition() > EncoderPos.retractRight.value){

From d8ec3c5d4e48b26f61dbe9434c1d7b6dc2d7ee23 Mon Sep 17 00:00:00 2001
From: CoolSpy3 <coolspythree@gmail.com>
Date: Sun, 2 Oct 2022 19:50:03 -0700
Subject: [PATCH 28/28] Apply suggestions from code review

Remove unnecessary imports.
Revert kDesiredExtendSpeedInps to its original value
Switch motor indexes to 0 1 2
---
 .../org/team199/robot2022/subsystems/Climber.java  | 14 ++++++--------
 1 file changed, 6 insertions(+), 8 deletions(-)

diff --git a/src/main/java/org/team199/robot2022/subsystems/Climber.java b/src/main/java/org/team199/robot2022/subsystems/Climber.java
index bfbe614..dea90b3 100644
--- a/src/main/java/org/team199/robot2022/subsystems/Climber.java
+++ b/src/main/java/org/team199/robot2022/subsystems/Climber.java
@@ -12,8 +12,6 @@
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.RelativeEncoder;
 import java.util.function.DoubleConsumer;
-import java.util.Hashtable;
-import java.util.Dictionary;
 
 import org.team199.robot2022.Constants;
 
@@ -22,7 +20,7 @@ public class Climber extends SubsystemBase {
     private static final double kNEOFreeSpeedRPM = 5680;
     private static final double kDiameterIn = 1;
     private static final double kDesiredRetractSpeedInps = 1;
-    private static final double kDesiredExtendSpeedInps = 6;
+    private static final double kDesiredExtendSpeedInps = 24*.6;
     private static final boolean leftInverted = false;
 
     private static final double gearing = 9;
@@ -38,9 +36,9 @@ public class Climber extends SubsystemBase {
     private boolean keepPosition = true;
     private double holdTolerance = 0.05;
 
-    public static final int leftMotor = -1;//because someone requested this and there's no point slapping them in an enum
-    public static final int bothMotors = 0;
-    public static final int rightMotor = 1;
+    public static final int leftMotor = 0;//because someone requested this and there's no point slapping them in an enum
+    public static final int bothMotors = 1;
+    public static final int rightMotor = 2;
 
 
     private final CANSparkMax left = MotorControllerFactory.createSparkMax(Constants.DrivePorts.kClimberLeft, TemperatureLimit.NEO);
@@ -119,11 +117,11 @@ public void keepZeroed() {
     //motor = -1 left or 1 right or 0 both
     public void resetEncodersTo(EncoderPos posEnum,int motor){
 			// setEncoder[motor+1].accept(dEncoderPos.get(pos));
-      setEncoder[motor+1].accept(posEnum.value);
+      setEncoder[motor].accept(posEnum.value);
     }
 
     public void moveMotors(MotorSpeed speedEnum, int motor){
-      setMotor[motor+1].accept(speedEnum.value);
+      setMotor[motor].accept(speedEnum.value);
     }
 
     public void stopMotors(int motor){