From 5c6d390710eddf1f6cdf749f4a207516bf5c0e83 Mon Sep 17 00:00:00 2001
From: SumedhP <43325361+SumedhP@users.noreply.github.com>
Date: Thu, 2 Jun 2022 17:29:26 -0700
Subject: [PATCH 01/25] Remy doing some outreach magic

---
 src/main/java/frc/team2412/robot/Controls.java              | 4 ++--
 src/main/java/frc/team2412/robot/Subsystems.java            | 6 +++---
 .../java/frc/team2412/robot/subsystem/ShooterSubsystem.java | 5 +++++
 3 files changed, 10 insertions(+), 5 deletions(-)

diff --git a/src/main/java/frc/team2412/robot/Controls.java b/src/main/java/frc/team2412/robot/Controls.java
index 35517330..7a9f94b9 100644
--- a/src/main/java/frc/team2412/robot/Controls.java
+++ b/src/main/java/frc/team2412/robot/Controls.java
@@ -145,11 +145,11 @@ public void bindShooterControls() {
         BooleanSupplier b = driveController.getDPadButton(Direction.UP)::get;
 
         driveController.getDPadButton(Direction.DOWN).whenPressed(
-                new ShooterHoodRPMCommand(subsystems.shooterSubsystem, 1400, 35).withInterrupt(b)
+                new ShooterHoodRPMCommand(subsystems.shooterSubsystem, 1500, 20).withInterrupt(b)
                         .alongWith(new InstantCommand(() -> subsystems.shooterSubsystem.setTurretAngle(-90))));
 
         driveController.getDPadButton(Direction.LEFT).whenPressed(
-                new ShooterHoodRPMCommand(subsystems.shooterSubsystem, 2400, 9.3).withInterrupt(b)
+                new ShooterHoodRPMCommand(subsystems.shooterSubsystem, 500, 15).withInterrupt(b)
                         .alongWith(new InstantCommand(() -> subsystems.shooterSubsystem.setTurretAngle(-90))));
 
         driveController.getDPadButton(Direction.RIGHT).whenPressed(
diff --git a/src/main/java/frc/team2412/robot/Subsystems.java b/src/main/java/frc/team2412/robot/Subsystems.java
index 1319695f..87c8ffff 100644
--- a/src/main/java/frc/team2412/robot/Subsystems.java
+++ b/src/main/java/frc/team2412/robot/Subsystems.java
@@ -7,18 +7,18 @@
 
 public class Subsystems implements Loggable {
     public static class SubsystemConstants {
-        public static final boolean CLIMB_ENABLED = true;
+        public static final boolean CLIMB_ENABLED = false;
 
         public static final boolean POST_CLIMB_ENABLED = false;
 
         public static final boolean DRIVE_ENABLED = true;
-        public static final boolean DRIVER_VIS_ENABLED = true;
+        public static final boolean DRIVER_VIS_ENABLED = false;
         public static final boolean SHOOTER_VISION_ENABLED = true;
         public static final boolean INDEX_ENABLED = true;
         public static final boolean INTAKE_ENABLED = true;
         public static final boolean SHOOTER_ENABLED = true;
 
-        public static final boolean COMPRESSOR_ENABLED = true;
+        public static final boolean COMPRESSOR_ENABLED = false;
     }
 
     public ClimbSubsystem climbSubsystem;
diff --git a/src/main/java/frc/team2412/robot/subsystem/ShooterSubsystem.java b/src/main/java/frc/team2412/robot/subsystem/ShooterSubsystem.java
index 9de0c124..0a574c75 100644
--- a/src/main/java/frc/team2412/robot/subsystem/ShooterSubsystem.java
+++ b/src/main/java/frc/team2412/robot/subsystem/ShooterSubsystem.java
@@ -187,6 +187,10 @@ private void configMotors() {
 
     @Override
     public void periodic() {
+        if (shooterOverride) {
+            setFlywheelRPM(flywheelTestRPM);
+            setHoodAngle(hoodTestAngle);
+        }
     }
 
     public void simInit(PhysicsSim sim) {
@@ -415,6 +419,7 @@ public void resetHoodEncoder(boolean reset) {
     public void setTurretAngle(double angle) {
 
         if (isTurretAtAngle(angle) || turretDisable) {
+            turretMotor.stopMotor();
             return;
         }
 

From ced29194ab31af64be312e3b82f685611c117bff Mon Sep 17 00:00:00 2001
From: SumedhP <43325361+SumedhP@users.noreply.github.com>
Date: Sat, 4 Jun 2022 16:36:28 -0700
Subject: [PATCH 02/25] Remy did the fix for drivebase

---
 src/main/java/frc/team2412/robot/Robot.java | 7 +++++++
 1 file changed, 7 insertions(+)

diff --git a/src/main/java/frc/team2412/robot/Robot.java b/src/main/java/frc/team2412/robot/Robot.java
index fb8649a1..c348d9ba 100644
--- a/src/main/java/frc/team2412/robot/Robot.java
+++ b/src/main/java/frc/team2412/robot/Robot.java
@@ -310,4 +310,11 @@ public void disabledInit() {
             subsystems.shooterSubsystem.stopFlywheel();
         }
     }
+
+    @Override
+    public void disabledPeriodic(){
+        if(subsystems.drivebaseSubsystem != null){
+            subsystems.drivebaseSubsystem.setToX();
+        }
+    }
 }

From 4983e170321c9f47dd58fb7e3d01d4f342fd8207 Mon Sep 17 00:00:00 2001
From: SumedhP <43325361+SumedhP@users.noreply.github.com>
Date: Sat, 4 Jun 2022 16:38:00 -0700
Subject: [PATCH 03/25] Remy undoing the outreach magic

---
 src/main/java/frc/team2412/robot/Controls.java   | 4 ++--
 src/main/java/frc/team2412/robot/Subsystems.java | 4 ++--
 2 files changed, 4 insertions(+), 4 deletions(-)

diff --git a/src/main/java/frc/team2412/robot/Controls.java b/src/main/java/frc/team2412/robot/Controls.java
index 7a9f94b9..35517330 100644
--- a/src/main/java/frc/team2412/robot/Controls.java
+++ b/src/main/java/frc/team2412/robot/Controls.java
@@ -145,11 +145,11 @@ public void bindShooterControls() {
         BooleanSupplier b = driveController.getDPadButton(Direction.UP)::get;
 
         driveController.getDPadButton(Direction.DOWN).whenPressed(
-                new ShooterHoodRPMCommand(subsystems.shooterSubsystem, 1500, 20).withInterrupt(b)
+                new ShooterHoodRPMCommand(subsystems.shooterSubsystem, 1400, 35).withInterrupt(b)
                         .alongWith(new InstantCommand(() -> subsystems.shooterSubsystem.setTurretAngle(-90))));
 
         driveController.getDPadButton(Direction.LEFT).whenPressed(
-                new ShooterHoodRPMCommand(subsystems.shooterSubsystem, 500, 15).withInterrupt(b)
+                new ShooterHoodRPMCommand(subsystems.shooterSubsystem, 2400, 9.3).withInterrupt(b)
                         .alongWith(new InstantCommand(() -> subsystems.shooterSubsystem.setTurretAngle(-90))));
 
         driveController.getDPadButton(Direction.RIGHT).whenPressed(
diff --git a/src/main/java/frc/team2412/robot/Subsystems.java b/src/main/java/frc/team2412/robot/Subsystems.java
index 87c8ffff..fd7c395c 100644
--- a/src/main/java/frc/team2412/robot/Subsystems.java
+++ b/src/main/java/frc/team2412/robot/Subsystems.java
@@ -7,7 +7,7 @@
 
 public class Subsystems implements Loggable {
     public static class SubsystemConstants {
-        public static final boolean CLIMB_ENABLED = false;
+        public static final boolean CLIMB_ENABLED = true;
 
         public static final boolean POST_CLIMB_ENABLED = false;
 
@@ -18,7 +18,7 @@ public static class SubsystemConstants {
         public static final boolean INTAKE_ENABLED = true;
         public static final boolean SHOOTER_ENABLED = true;
 
-        public static final boolean COMPRESSOR_ENABLED = false;
+        public static final boolean COMPRESSOR_ENABLED = true;
     }
 
     public ClimbSubsystem climbSubsystem;

From e6a52e6fc0b4554217753fc825e9d4a5a4733cc9 Mon Sep 17 00:00:00 2001
From: SumedhP <43325361+SumedhP@users.noreply.github.com>
Date: Sat, 4 Jun 2022 16:45:38 -0700
Subject: [PATCH 04/25] Guess what, Remy made code spotless

---
 src/main/java/frc/team2412/robot/Robot.java | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc/team2412/robot/Robot.java b/src/main/java/frc/team2412/robot/Robot.java
index c348d9ba..3e4aef38 100644
--- a/src/main/java/frc/team2412/robot/Robot.java
+++ b/src/main/java/frc/team2412/robot/Robot.java
@@ -312,8 +312,8 @@ public void disabledInit() {
     }
 
     @Override
-    public void disabledPeriodic(){
-        if(subsystems.drivebaseSubsystem != null){
+    public void disabledPeriodic() {
+        if (subsystems.drivebaseSubsystem != null) {
             subsystems.drivebaseSubsystem.setToX();
         }
     }

From cfbe2552c196615c5b1273b03aa0a8a0db3ca730 Mon Sep 17 00:00:00 2001
From: SumedhP <43325361+SumedhP@users.noreply.github.com>
Date: Sat, 18 Jun 2022 19:48:35 -0700
Subject: [PATCH 05/25] Remy made motors easier to deal with

---
 .../team2412/robot/util/MotorStuff/Motor.java | 39 ++++++++
 .../util/MotorStuff/RobototesCANSparkMax.java | 86 +++++++++++++++++
 .../util/MotorStuff/RobototesTalonFX.java     | 95 +++++++++++++++++++
 3 files changed, 220 insertions(+)
 create mode 100644 src/main/java/frc/team2412/robot/util/MotorStuff/Motor.java
 create mode 100644 src/main/java/frc/team2412/robot/util/MotorStuff/RobototesCANSparkMax.java
 create mode 100644 src/main/java/frc/team2412/robot/util/MotorStuff/RobototesTalonFX.java

diff --git a/src/main/java/frc/team2412/robot/util/MotorStuff/Motor.java b/src/main/java/frc/team2412/robot/util/MotorStuff/Motor.java
new file mode 100644
index 00000000..d64abf98
--- /dev/null
+++ b/src/main/java/frc/team2412/robot/util/MotorStuff/Motor.java
@@ -0,0 +1,39 @@
+package frc.team2412.robot.util.MotorStuff;
+
+import edu.wpi.first.wpilibj.motorcontrol.MotorController;
+
+public interface Motor extends MotorController {
+
+    public void setPosition(double value);
+
+    public void setVelocity(double value);
+
+    public void setVelocity(double value, double feedforward);
+
+    public void setVoltage(double voltage);
+
+    public void setToCoastMode();
+
+    public void setToBrakeMode();
+
+    public void setCurrentLimit(double amp);
+
+    public void setCurrentLimit(double amp, double timeMs);
+
+    public double getEncoderPosition();
+
+    public double getEncoderVelocity();
+
+    public void setEncoderPosition(double position);
+
+    public void resetEncoder();
+
+    public void setP(double value);
+
+    public void setI(double value);
+
+    public void setD(double value);
+
+    public void setF(double value);
+
+}
diff --git a/src/main/java/frc/team2412/robot/util/MotorStuff/RobototesCANSparkMax.java b/src/main/java/frc/team2412/robot/util/MotorStuff/RobototesCANSparkMax.java
new file mode 100644
index 00000000..86eb09dc
--- /dev/null
+++ b/src/main/java/frc/team2412/robot/util/MotorStuff/RobototesCANSparkMax.java
@@ -0,0 +1,86 @@
+package frc.team2412.robot.util.MotorStuff;
+
+import com.revrobotics.CANSparkMax;
+
+public class RobototesCANSparkMax extends CANSparkMax implements Motor {
+
+    public RobototesCANSparkMax(int deviceId) {
+        super(deviceId, MotorType.kBrushless);
+    }
+
+    @Override
+    public void setPosition(double value) {
+        getPIDController().setReference(value, ControlType.kPosition);
+    }
+
+    @Override
+    public void setVelocity(double value) {
+        getPIDController().setReference(value, ControlType.kVelocity);
+    }
+
+    @Override
+    public void setVelocity(double value, double feedforward) {
+        getPIDController().setReference(value, ControlType.kVelocity, 0, feedforward);
+    }
+
+    @Override
+    public void setToCoastMode() {
+        setIdleMode(IdleMode.kCoast);
+    }
+
+    @Override
+    public void setToBrakeMode() {
+        setIdleMode(IdleMode.kBrake);
+    }
+
+    @Override
+    public void setCurrentLimit(double amp) {
+        setSmartCurrentLimit((int) amp);
+    }
+
+    @Override
+    public void setCurrentLimit(double amp, double timeMs) {
+        setCurrentLimit(amp);
+    }
+
+    @Override
+    public double getEncoderPosition() {
+        return getEncoder().getPosition();
+    }
+
+    @Override
+    public double getEncoderVelocity() {
+        return getEncoder().getVelocity() * 2048 / 600;
+    }
+
+    @Override
+    public void setEncoderPosition(double position) {
+        getEncoder().setPosition(position);
+    }
+
+    @Override
+    public void resetEncoder() {
+        setEncoderPosition(0);
+    }
+
+    @Override
+    public void setP(double value) {
+        getPIDController().setP(value);
+    }
+
+    @Override
+    public void setI(double value) {
+        getPIDController().setI(value);
+    }
+
+    @Override
+    public void setD(double value) {
+        getPIDController().setD(value);
+    }
+
+    @Override
+    public void setF(double value) {
+        getPIDController().setFF(value);
+    }
+
+}
diff --git a/src/main/java/frc/team2412/robot/util/MotorStuff/RobototesTalonFX.java b/src/main/java/frc/team2412/robot/util/MotorStuff/RobototesTalonFX.java
new file mode 100644
index 00000000..07685011
--- /dev/null
+++ b/src/main/java/frc/team2412/robot/util/MotorStuff/RobototesTalonFX.java
@@ -0,0 +1,95 @@
+package frc.team2412.robot.util.MotorStuff;
+
+import static com.ctre.phoenix.motorcontrol.ControlMode.*;
+import static com.ctre.phoenix.motorcontrol.DemandType.*;
+import static com.ctre.phoenix.motorcontrol.NeutralMode.*;
+
+import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration;
+import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
+
+public class RobototesTalonFX extends WPI_TalonFX implements Motor {
+
+    public RobototesTalonFX(int deviceNumber) {
+        super(deviceNumber);
+    }
+
+    public RobototesTalonFX(int deviceNumber, String canbus) {
+        super(deviceNumber, canbus);
+    }
+
+    @Override
+    public void setPosition(double value) {
+        set(Position, value);
+    }
+
+    @Override
+    public void setVelocity(double value) {
+        set(Velocity, value);
+    }
+
+    @Override
+    public void setVelocity(double value, double feedforward) {
+        set(Velocity, value, ArbitraryFeedForward, feedforward);
+    }
+
+    @Override
+    public void setToCoastMode() {
+        setNeutralMode(Coast);
+    }
+
+    @Override
+    public void setToBrakeMode() {
+        setNeutralMode(Brake);
+    }
+
+    @Override
+    public void setCurrentLimit(double amp) {
+        setCurrentLimit(amp, 0);
+    }
+
+    @Override
+    public void setCurrentLimit(double amp, double timeSeconds) {
+        configSupplyCurrentLimit(new SupplyCurrentLimitConfiguration(true, amp, 0, timeSeconds));
+    }
+
+    @Override
+    public double getEncoderPosition() {
+        return getSelectedSensorPosition();
+    }
+
+    @Override
+    public double getEncoderVelocity() {
+        return getSelectedSensorVelocity();
+    }
+
+    @Override
+    public void setEncoderPosition(double position) {
+        setSelectedSensorPosition(position);
+    }
+
+    @Override
+    public void resetEncoder() {
+        setEncoderPosition(0);
+    }
+
+    @Override
+    public void setP(double value) {
+        config_kP(0, value);
+    }
+
+    @Override
+    public void setI(double value) {
+        config_kI(0, value);
+    }
+
+    @Override
+    public void setD(double value) {
+        config_kD(0, value);
+    }
+
+    @Override
+    public void setF(double value) {
+        config_kF(0, value);
+    }
+
+}

From 1a5f0920e8291665679e5e0a1ed17f9558af3af8 Mon Sep 17 00:00:00 2001
From: SumedhP <43325361+SumedhP@users.noreply.github.com>
Date: Sat, 18 Jun 2022 19:49:52 -0700
Subject: [PATCH 06/25] Remy made proof in concept????

---
 .../java/frc/team2412/robot/subsystem/ClimbSubsystem.java   | 6 ++++--
 1 file changed, 4 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc/team2412/robot/subsystem/ClimbSubsystem.java b/src/main/java/frc/team2412/robot/subsystem/ClimbSubsystem.java
index 9eff2ea2..5bbc4285 100644
--- a/src/main/java/frc/team2412/robot/subsystem/ClimbSubsystem.java
+++ b/src/main/java/frc/team2412/robot/subsystem/ClimbSubsystem.java
@@ -13,6 +13,8 @@
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.team2412.robot.sim.PhysicsSim;
+import frc.team2412.robot.util.MotorStuff.Motor;
+import frc.team2412.robot.util.MotorStuff.RobototesTalonFX;
 import io.github.oblarg.oblog.Loggable;
 import io.github.oblarg.oblog.annotations.Config;
 import io.github.oblarg.oblog.annotations.Log;
@@ -59,14 +61,14 @@ public static class ClimbConstants {
     }
 
     @Log.MotorController
-    private final WPI_TalonFX motor;
+    private final RobototesTalonFX motor;
 
     // For use in the simulationPeriodic to get encoder position
     private double lastUpdatedTime = Timer.getFPGATimestamp();
 
     public ClimbSubsystem() {
         setName("ClimbSubsystem");
-        motor = new WPI_TalonFX(CLIMB_FIXED_MOTOR);
+        motor = new RobototesTalonFX(CLIMB_FIXED_MOTOR);
 
         // Configure motor soft limits, current limits and peak outputs
         TalonFXConfiguration motorConfig = new TalonFXConfiguration();

From b516835546a2a78a9db058ca6668972b4e857549 Mon Sep 17 00:00:00 2001
From: SumedhP <43325361+SumedhP@users.noreply.github.com>
Date: Fri, 24 Jun 2022 13:47:00 -0700
Subject: [PATCH 07/25] Remy made code better

---
 .../robot/subsystem/ClimbSubsystem.java       | 11 ++-
 .../robot/subsystem/DrivebaseSubsystem.java   | 52 ++++-------
 ...RobototesTalonFX.java => FalconMotor.java} |  6 +-
 .../util/MotorStuff/RobototesCANSparkMax.java | 86 -------------------
 4 files changed, 27 insertions(+), 128 deletions(-)
 rename src/main/java/frc/team2412/robot/util/MotorStuff/{RobototesTalonFX.java => FalconMotor.java} (91%)
 delete mode 100644 src/main/java/frc/team2412/robot/util/MotorStuff/RobototesCANSparkMax.java

diff --git a/src/main/java/frc/team2412/robot/subsystem/ClimbSubsystem.java b/src/main/java/frc/team2412/robot/subsystem/ClimbSubsystem.java
index 5bbc4285..e05df74b 100644
--- a/src/main/java/frc/team2412/robot/subsystem/ClimbSubsystem.java
+++ b/src/main/java/frc/team2412/robot/subsystem/ClimbSubsystem.java
@@ -8,13 +8,11 @@
 import com.ctre.phoenix.motorcontrol.NeutralMode;
 import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration;
 import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration;
-import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
 
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.team2412.robot.sim.PhysicsSim;
-import frc.team2412.robot.util.MotorStuff.Motor;
-import frc.team2412.robot.util.MotorStuff.RobototesTalonFX;
+import frc.team2412.robot.util.MotorStuff.FalconMotor;
 import io.github.oblarg.oblog.Loggable;
 import io.github.oblarg.oblog.annotations.Config;
 import io.github.oblarg.oblog.annotations.Log;
@@ -38,7 +36,8 @@ public static class ClimbConstants {
         public static final double RETRACTION_I = 0;
         public static final double RETRACTION_D = 0;
         public static final double RETRACTION_F = 0.18;
-        // This is based on the minimum amount of motor power need to keep climb arm in place, need to test
+        // This is based on the minimum amount of motor power need to keep climb arm in
+        // place, need to test
 
         // Relating to physical climb structure things
         // was previously mid
@@ -61,14 +60,14 @@ public static class ClimbConstants {
     }
 
     @Log.MotorController
-    private final RobototesTalonFX motor;
+    private final FalconMotor motor;
 
     // For use in the simulationPeriodic to get encoder position
     private double lastUpdatedTime = Timer.getFPGATimestamp();
 
     public ClimbSubsystem() {
         setName("ClimbSubsystem");
-        motor = new RobototesTalonFX(CLIMB_FIXED_MOTOR);
+        motor = new FalconMotor(CLIMB_FIXED_MOTOR);
 
         // Configure motor soft limits, current limits and peak outputs
         TalonFXConfiguration motorConfig = new TalonFXConfiguration();
diff --git a/src/main/java/frc/team2412/robot/subsystem/DrivebaseSubsystem.java b/src/main/java/frc/team2412/robot/subsystem/DrivebaseSubsystem.java
index b7458276..69841ddb 100644
--- a/src/main/java/frc/team2412/robot/subsystem/DrivebaseSubsystem.java
+++ b/src/main/java/frc/team2412/robot/subsystem/DrivebaseSubsystem.java
@@ -1,48 +1,34 @@
 package frc.team2412.robot.subsystem;
 
 import static frc.team2412.robot.Hardware.*;
+import static frc.team2412.robot.subsystem.DrivebaseSubsystem.DriveConstants.*;
 
-import com.google.errorprone.annotations.concurrent.GuardedBy;
-import com.swervedrivespecialties.swervelib.SwerveModule;
-import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.kinematics.ChassisSpeeds;
-import edu.wpi.first.math.util.Units;
-import edu.wpi.first.networktables.NetworkTableEntry;
-import edu.wpi.first.wpilibj.RobotController;
-import edu.wpi.first.wpilibj.SerialPort;
-import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
-import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
-import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
-import edu.wpi.first.wpilibj.smartdashboard.Field2d;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc.team2412.robot.Hardware;
-import frc.team2412.robot.Robot;
-import frc.team2412.robot.util.GeoConvertor;
-import frc.team2412.robot.util.PFFController;
-import frc.team2412.robot.util.VectorSlewLimiter;
-import io.github.oblarg.oblog.annotations.Config;
+import java.util.*;
 
 import org.frcteam2910.common.control.*;
 import org.frcteam2910.common.drivers.Gyroscope;
-import org.frcteam2910.common.kinematics.ChassisVelocity;
-import org.frcteam2910.common.kinematics.SwerveKinematics;
-import org.frcteam2910.common.kinematics.SwerveOdometry;
-import org.frcteam2910.common.math.RigidTransform2;
-import org.frcteam2910.common.math.Rotation2;
-import org.frcteam2910.common.math.Vector2;
+import org.frcteam2910.common.kinematics.*;
+import org.frcteam2910.common.math.*;
 import org.frcteam2910.common.robot.UpdateManager;
-import org.frcteam2910.common.robot.drivers.NavX;
-import org.frcteam2910.common.robot.drivers.PigeonTwo;
+import org.frcteam2910.common.robot.drivers.*;
 import org.frcteam2910.common.util.*;
+import org.frcteam2910.common.util.InterpolatingTreeMap;
 
-import java.util.Map;
-import java.util.Optional;
+import com.google.errorprone.annotations.concurrent.GuardedBy;
+import com.swervedrivespecialties.swervelib.SwerveModule;
 
+import edu.wpi.first.math.geometry.*;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.wpilibj.*;
+import edu.wpi.first.wpilibj.shuffleboard.*;
+import edu.wpi.first.wpilibj.smartdashboard.*;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.team2412.robot.*;
+import frc.team2412.robot.util.*;
 import io.github.oblarg.oblog.Loggable;
-
-import static frc.team2412.robot.subsystem.DrivebaseSubsystem.DriveConstants.*;
+import io.github.oblarg.oblog.annotations.Config;
 
 public class DrivebaseSubsystem extends SubsystemBase implements UpdateManager.Updatable, Loggable {
 
diff --git a/src/main/java/frc/team2412/robot/util/MotorStuff/RobototesTalonFX.java b/src/main/java/frc/team2412/robot/util/MotorStuff/FalconMotor.java
similarity index 91%
rename from src/main/java/frc/team2412/robot/util/MotorStuff/RobototesTalonFX.java
rename to src/main/java/frc/team2412/robot/util/MotorStuff/FalconMotor.java
index 07685011..5278ab27 100644
--- a/src/main/java/frc/team2412/robot/util/MotorStuff/RobototesTalonFX.java
+++ b/src/main/java/frc/team2412/robot/util/MotorStuff/FalconMotor.java
@@ -7,13 +7,13 @@
 import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration;
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
 
-public class RobototesTalonFX extends WPI_TalonFX implements Motor {
+public class FalconMotor extends WPI_TalonFX implements Motor {
 
-    public RobototesTalonFX(int deviceNumber) {
+    public FalconMotor(int deviceNumber) {
         super(deviceNumber);
     }
 
-    public RobototesTalonFX(int deviceNumber, String canbus) {
+    public FalconMotor(int deviceNumber, String canbus) {
         super(deviceNumber, canbus);
     }
 
diff --git a/src/main/java/frc/team2412/robot/util/MotorStuff/RobototesCANSparkMax.java b/src/main/java/frc/team2412/robot/util/MotorStuff/RobototesCANSparkMax.java
deleted file mode 100644
index 86eb09dc..00000000
--- a/src/main/java/frc/team2412/robot/util/MotorStuff/RobototesCANSparkMax.java
+++ /dev/null
@@ -1,86 +0,0 @@
-package frc.team2412.robot.util.MotorStuff;
-
-import com.revrobotics.CANSparkMax;
-
-public class RobototesCANSparkMax extends CANSparkMax implements Motor {
-
-    public RobototesCANSparkMax(int deviceId) {
-        super(deviceId, MotorType.kBrushless);
-    }
-
-    @Override
-    public void setPosition(double value) {
-        getPIDController().setReference(value, ControlType.kPosition);
-    }
-
-    @Override
-    public void setVelocity(double value) {
-        getPIDController().setReference(value, ControlType.kVelocity);
-    }
-
-    @Override
-    public void setVelocity(double value, double feedforward) {
-        getPIDController().setReference(value, ControlType.kVelocity, 0, feedforward);
-    }
-
-    @Override
-    public void setToCoastMode() {
-        setIdleMode(IdleMode.kCoast);
-    }
-
-    @Override
-    public void setToBrakeMode() {
-        setIdleMode(IdleMode.kBrake);
-    }
-
-    @Override
-    public void setCurrentLimit(double amp) {
-        setSmartCurrentLimit((int) amp);
-    }
-
-    @Override
-    public void setCurrentLimit(double amp, double timeMs) {
-        setCurrentLimit(amp);
-    }
-
-    @Override
-    public double getEncoderPosition() {
-        return getEncoder().getPosition();
-    }
-
-    @Override
-    public double getEncoderVelocity() {
-        return getEncoder().getVelocity() * 2048 / 600;
-    }
-
-    @Override
-    public void setEncoderPosition(double position) {
-        getEncoder().setPosition(position);
-    }
-
-    @Override
-    public void resetEncoder() {
-        setEncoderPosition(0);
-    }
-
-    @Override
-    public void setP(double value) {
-        getPIDController().setP(value);
-    }
-
-    @Override
-    public void setI(double value) {
-        getPIDController().setI(value);
-    }
-
-    @Override
-    public void setD(double value) {
-        getPIDController().setD(value);
-    }
-
-    @Override
-    public void setF(double value) {
-        getPIDController().setFF(value);
-    }
-
-}

From 91519703433d095328b4a018fe7c145bbb04bbd3 Mon Sep 17 00:00:00 2001
From: SumedhP <43325361+SumedhP@users.noreply.github.com>
Date: Fri, 24 Jun 2022 14:24:50 -0700
Subject: [PATCH 08/25] =?UTF-8?q?Remy=20started=20writing=20docs=20?=
 =?UTF-8?q?=F0=9F=98=B2=F0=9F=A4=AF=F0=9F=A4=AF=F0=9F=A4=AF=F0=9F=A4=AF?=
 =?UTF-8?q?=F0=9F=A4=AF=F0=9F=A4=AF?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 .../robot/util/MotorStuff/FalconMotor.java    | 58 ++++++++++++++++++-
 1 file changed, 56 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc/team2412/robot/util/MotorStuff/FalconMotor.java b/src/main/java/frc/team2412/robot/util/MotorStuff/FalconMotor.java
index 5278ab27..6d8324e0 100644
--- a/src/main/java/frc/team2412/robot/util/MotorStuff/FalconMotor.java
+++ b/src/main/java/frc/team2412/robot/util/MotorStuff/FalconMotor.java
@@ -9,47 +9,101 @@
 
 public class FalconMotor extends WPI_TalonFX implements Motor {
 
+    /**
+     * Constructor for motor
+	 * @param deviceNumber device ID of motor controller (this value is assigned in pheonix tuner)
+     */
     public FalconMotor(int deviceNumber) {
         super(deviceNumber);
     }
 
+    /**
+	 * Constructor for motor. Use in case the motor is on a different CAN network.
+     * This is used primarily for any devices on a CANivore network
+	 * 
+     * @param deviceNumber device ID of motor controller (this value is assigned in pheonix tuner)
+	 * @param canbus Name of the CANbus (Canivore name can be found in pheonix tuner)
+	 * 
+	 */
     public FalconMotor(int deviceNumber, String canbus) {
         super(deviceNumber, canbus);
     }
 
+    /**
+     * Sets the motor to the target position.
+     * Uses a PID loop and requires PID values to be nonzero
+     * 
+     * @param value position setpoint for motor to go to. Units are in encoder ticks.
+     */
     @Override
     public void setPosition(double value) {
         set(Position, value);
     }
 
+    /**
+     * Sets the motor to go target velocity.
+     * Uses a PID loop and requires PID values to be nonzero
+     * 
+     * @param value velocity setpoint for motor to go to. Units are in encoder ticks per 100ms.
+     */
     @Override
     public void setVelocity(double value) {
         set(Velocity, value);
     }
 
+     /**
+     * Sets the motor to go target velocity using feedforward term as an offset to motor power.
+     * Uses a PID loop and requires PID values to be nonzero
+     * Use this method when PID requires a constant force i.e. gravity compensation on climber or hood
+     * 
+     * @param value velocity setpoint for motor to go to. Units are in encoder ticks per 100ms.
+     * @param feedforward feedforward value to be used as baseline motor power. Value must be between [-1,1]
+     * 
+     * @see <a href="https://docs.ctre-phoenix.com/en/stable/ch16_ClosedLoop.html#arbitrary-feed-forward">CTRE Documentation on feedforward</a>
+     */
     @Override
     public void setVelocity(double value, double feedforward) {
         set(Velocity, value, ArbitraryFeedForward, feedforward);
     }
 
+    /**
+     * Sets motor to coast, allowing motor to freely spin when acted upon other forces.
+     * Motor will provide no resistance to rotation.
+     * If wanting motor to stay in place, use brake mode
+     */
     @Override
     public void setToCoastMode() {
         setNeutralMode(Coast);
     }
 
+    /**
+     * Sets motor to brake, causing motor to resist outside forces.
+     * Using this will cause the motor to apply electrical resistance to rotation.
+     * If wanting motor to spin freely, use coast mode
+     */
     @Override
     public void setToBrakeMode() {
         setNeutralMode(Brake);
     }
 
+    /**
+     * Creates a current limit, preventing how much a motor can draw.
+     * @param amp maximum amount of amperage the motor can draw
+     */
     @Override
     public void setCurrentLimit(double amp) {
-        setCurrentLimit(amp, 0);
+        configSupplyCurrentLimit(new SupplyCurrentLimitConfiguration(true, amp, 0, 0));
     }
 
+    /**
+     * Creates a current limit, preventing how much a motor can draw.
+     * 
+     * @param amp maximum amount of amperage the motor can draw
+     * @param timeSeconds how long the motor can go above threshold before being limited
+     */
     @Override
     public void setCurrentLimit(double amp, double timeSeconds) {
-        configSupplyCurrentLimit(new SupplyCurrentLimitConfiguration(true, amp, 0, timeSeconds));
+        configSupplyCurrentLimit(new SupplyCurrentLimitConfiguration(true, amp, amp, timeSeconds));
     }
 
     @Override

From a576e37f74eaf6ca3428cc59dbf1dfe37739aae0 Mon Sep 17 00:00:00 2001
From: SumedhP <43325361+SumedhP@users.noreply.github.com>
Date: Fri, 24 Jun 2022 14:41:57 -0700
Subject: [PATCH 09/25] =?UTF-8?q?Remy=20finished=20writing=20docs=20?=
 =?UTF-8?q?=F0=9F=A4=AF=F0=9F=A4=AF=F0=9F=A4=AF=F0=9F=98=8D=F0=9F=98=8D?=
 =?UTF-8?q?=F0=9F=98=8D?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 .../robot/util/MotorStuff/FalconMotor.java    | 47 +++++++++++++++++--
 .../team2412/robot/util/MotorStuff/Motor.java |  2 -
 2 files changed, 42 insertions(+), 7 deletions(-)

diff --git a/src/main/java/frc/team2412/robot/util/MotorStuff/FalconMotor.java b/src/main/java/frc/team2412/robot/util/MotorStuff/FalconMotor.java
index 6d8324e0..4693e98f 100644
--- a/src/main/java/frc/team2412/robot/util/MotorStuff/FalconMotor.java
+++ b/src/main/java/frc/team2412/robot/util/MotorStuff/FalconMotor.java
@@ -106,41 +106,78 @@ public void setCurrentLimit(double amp, double timeSeconds) {
         configSupplyCurrentLimit(new SupplyCurrentLimitConfiguration(true, amp, amp, timeSeconds));
     }
 
+    /**
+	 * Get the encoder position. This returns a value in encoder ticks. 2048 encoder ticks result in 1 motor revolution
+	 *
+	 * @return Position of encoder in encoder ticks.
+	 */
     @Override
     public double getEncoderPosition() {
         return getSelectedSensorPosition();
     }
 
+    /**
+	 * Get the encoder velocity. This returns the rate in which the motor is spinning.
+     * The value in encoder ticks per 100ms. 2048 encoder ticks result in 1 motor revolution
+	 *
+	 * @return velocity of encoder in encoder ticks.
+	 */
     @Override
     public double getEncoderVelocity() {
         return getSelectedSensorVelocity();
     }
 
+    /**
+	 * Sets the encoder position to the given value.
+	 *
+	 * @param position Position to set for the selected sensor, value is in encoder ticks.
+	 */
     @Override
     public void setEncoderPosition(double position) {
         setSelectedSensorPosition(position);
     }
 
+    /**
+	 * Sets the encoder position to 0.
+	 *
+	 */
     @Override
     public void resetEncoder() {
         setEncoderPosition(0);
     }
 
+    /**
+	 * Sets the 'P' constant for a PID loop.
+	 * In a loop, this value is multiplied by closed loop error.
+	 *
+	 * @param value Value of the P constant.
+	 */
     @Override
     public void setP(double value) {
         config_kP(0, value);
     }
 
-    @Override
-    public void setI(double value) {
-        config_kI(0, value);
-    }
-
+    /**
+	 * Sets the 'D' constant for a PID loop.
+	 *
+	 * This is multiplied by derivative error (slope of error over the last 1ms)
+     * 
+	 * @param value Value of the D constant.
+	 */
     @Override
     public void setD(double value) {
         config_kD(0, value);
     }
 
+    /**
+	 * Sets the 'F' constant in PID loop.
+     * This is a different value than arbitrary feedforward
+	 *
+	 * @see <a href="https://docs.ctre-phoenix.com/en/stable/ch16_ClosedLoop.html?highlight=kF#calculating-velocity-feed-forward-gain-kf">CTRE Documentation on F in PID</a>
+
+	 *
+	 * @param value Value of the F constant.
+	 */
     @Override
     public void setF(double value) {
         config_kF(0, value);
diff --git a/src/main/java/frc/team2412/robot/util/MotorStuff/Motor.java b/src/main/java/frc/team2412/robot/util/MotorStuff/Motor.java
index d64abf98..716cb128 100644
--- a/src/main/java/frc/team2412/robot/util/MotorStuff/Motor.java
+++ b/src/main/java/frc/team2412/robot/util/MotorStuff/Motor.java
@@ -30,8 +30,6 @@ public interface Motor extends MotorController {
 
     public void setP(double value);
 
-    public void setI(double value);
-
     public void setD(double value);
 
     public void setF(double value);

From 2b56e938df75fcb5bbcf5bfdf40672df403d4847 Mon Sep 17 00:00:00 2001
From: SumedhP <43325361+SumedhP@users.noreply.github.com>
Date: Fri, 24 Jun 2022 15:59:05 -0700
Subject: [PATCH 10/25] Remy made the implementation better

---
 .../robot/subsystem/ClimbSubsystem.java       |  46 +++---
 .../robot/util/MotorStuff/FalconMotor.java    | 150 +++++++++++-------
 .../team2412/robot/util/MotorStuff/Motor.java |   2 +
 3 files changed, 115 insertions(+), 83 deletions(-)

diff --git a/src/main/java/frc/team2412/robot/subsystem/ClimbSubsystem.java b/src/main/java/frc/team2412/robot/subsystem/ClimbSubsystem.java
index e05df74b..5a3400ab 100644
--- a/src/main/java/frc/team2412/robot/subsystem/ClimbSubsystem.java
+++ b/src/main/java/frc/team2412/robot/subsystem/ClimbSubsystem.java
@@ -1,21 +1,16 @@
 package frc.team2412.robot.subsystem;
 
-import static frc.team2412.robot.subsystem.ClimbSubsystem.ClimbConstants.*;
 import static frc.team2412.robot.Hardware.*;
+import static frc.team2412.robot.subsystem.ClimbSubsystem.ClimbConstants.*;
 
-import com.ctre.phoenix.motorcontrol.ControlMode;
-import com.ctre.phoenix.motorcontrol.DemandType;
-import com.ctre.phoenix.motorcontrol.NeutralMode;
 import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration;
-import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration;
 
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.team2412.robot.sim.PhysicsSim;
 import frc.team2412.robot.util.MotorStuff.FalconMotor;
 import io.github.oblarg.oblog.Loggable;
-import io.github.oblarg.oblog.annotations.Config;
-import io.github.oblarg.oblog.annotations.Log;
+import io.github.oblarg.oblog.annotations.*;
 
 public class ClimbSubsystem extends SubsystemBase implements Loggable {
 
@@ -69,17 +64,12 @@ public ClimbSubsystem() {
         setName("ClimbSubsystem");
         motor = new FalconMotor(CLIMB_FIXED_MOTOR);
 
-        // Configure motor soft limits, current limits and peak outputs
-        TalonFXConfiguration motorConfig = new TalonFXConfiguration();
-        motorConfig.forwardSoftLimitEnable = false;
-        motorConfig.reverseSoftLimitEnable = false;
-        motorConfig.forwardSoftLimitThreshold = MAX_ENCODER_TICKS;
-        motorConfig.reverseSoftLimitThreshold = MIN_ENCODER_TICKS;
-        motorConfig.supplyCurrLimit = MOTOR_CURRENT_LIMIT;
-        motor.configAllSettings(motorConfig);
-        motor.setNeutralMode(NeutralMode.Brake);
-
-        setPIDExtend(EXTENSION_P, EXTENSION_I, EXTENSION_D);
+        motor.setToBrakeMode();
+        motor.setCurrentLimit(40, 15);
+
+        motor.setP(EXTENSION_P);
+        motor.setD(EXTENSION_D);
+
         setPIDRetract(RETRACTION_P, RETRACTION_I, RETRACTION_D);
     }
 
@@ -100,7 +90,7 @@ public void simInit(PhysicsSim sim) {
      * Stop the dynamic climb arm motor, graciously
      *
      * @param stop
-     *            Whether to stop the motor
+     *             Whether to stop the motor
      */
     @Config(name = "Stop Fixed Motor")
     public void stopArm(boolean stop) {
@@ -131,10 +121,10 @@ public void retractArm() {
      * Set the position to which the motor will graciously follow
      *
      * @param value
-     *            The position to set the motor
+     *              The position to set the motor
      */
-    public void setMotor(double value, double feedForward) {
-        motor.set(ControlMode.Position, value, DemandType.ArbitraryFeedForward, feedForward);
+    public void setMotor(double value, double feedforward) {
+        motor.setPosition(value, feedforward);
     }
 
     public void setMotorSpeed(double speed) {
@@ -163,7 +153,7 @@ public void simulationPeriodic() {
      */
     @Log
     public double encoderPosition() {
-        return motor.getSelectedSensorPosition();
+        return motor.getEncoderPosition();
     }
 
     /**
@@ -183,12 +173,12 @@ public double climbHeightRemy() {
      * specified as true.
      *
      * @param reset
-     *            whether to reset the encoder
+     *              whether to reset the encoder
      */
     @Config.ToggleButton
     public void resetEncoder(boolean reset) {
         if (reset) {
-            motor.setSelectedSensorPosition(0);
+            motor.resetEncoder();
         }
     }
 
@@ -196,11 +186,11 @@ public void resetEncoder(boolean reset) {
      * Configure the motor PID (probably graciously)
      *
      * @param p
-     *            the P value to configure
+     *          the P value to configure
      * @param i
-     *            the I value to configure
+     *          the I value to configure
      * @param d
-     *            the D value to configure
+     *          the D value to configure
      */
     @Config(name = "PID extend")
     private void setPIDExtend(@Config(name = "EXTENSION P", defaultValueNumeric = EXTENSION_P) double p,
diff --git a/src/main/java/frc/team2412/robot/util/MotorStuff/FalconMotor.java b/src/main/java/frc/team2412/robot/util/MotorStuff/FalconMotor.java
index 4693e98f..be77a27c 100644
--- a/src/main/java/frc/team2412/robot/util/MotorStuff/FalconMotor.java
+++ b/src/main/java/frc/team2412/robot/util/MotorStuff/FalconMotor.java
@@ -11,20 +11,24 @@ public class FalconMotor extends WPI_TalonFX implements Motor {
 
     /**
      * Constructor for motor
-	 * @param deviceNumber device ID of motor controller (this value is assigned in pheonix tuner)
+     *
+     * @param deviceNumber
+     *            device ID of motor controller (this value is assigned in pheonix tuner)
      */
     public FalconMotor(int deviceNumber) {
         super(deviceNumber);
     }
 
     /**
-	 * Constructor for motor. Use in case the motor is on a different CAN network.
+     * Constructor for motor. Use in case the motor is on a different CAN network.
      * This is used primarily for any devices on a CANivore network
-	 * 
-     * @param deviceNumber device ID of motor controller (this value is assigned in pheonix tuner)
-	 * @param canbus Name of the CANbus (Canivore name can be found in pheonix tuner)
-	 * 
-	 */
+     *
+     * @param deviceNumber
+     *            device ID of motor controller (this value is assigned in pheonix tuner)
+     * @param canbus
+     *            Name of the CANbus (Canivore name can be found in pheonix tuner)
+     *
+     */
     public FalconMotor(int deviceNumber, String canbus) {
         super(deviceNumber, canbus);
     }
@@ -32,34 +36,59 @@ public FalconMotor(int deviceNumber, String canbus) {
     /**
      * Sets the motor to the target position.
      * Uses a PID loop and requires PID values to be nonzero
-     * 
-     * @param value position setpoint for motor to go to. Units are in encoder ticks.
+     *
+     * @param value
+     *            position setpoint for motor to go to. Units are in encoder ticks.
      */
     @Override
     public void setPosition(double value) {
         set(Position, value);
     }
 
+    /**
+     * Sets the motor to the target position using feedforward term as an offset to motor power.
+     * Uses a PID loop and requires PID values to be nonzero.
+     * Use this method when PID requires a constant force i.e. gravity compensation on climber or hood
+     *
+     * @param value
+     *            position setpoint for motor to go to. Units are in encoder ticks.
+     * @param feedforward
+     *            feedforward value to be used as baseline motor power. Value must be between [-1,1]
+     *
+     * @see <a href=
+     *      "https://docs.ctre-phoenix.com/en/stable/ch16_ClosedLoop.html#arbitrary-feed-forward">CTRE
+     *      Documentation on feedforward</a>
+     */
+    @Override
+    public void setPosition(double value, double feedforward) {
+        set(Position, value, ArbitraryFeedForward, feedforward);
+    }
+
     /**
      * Sets the motor to go target velocity.
      * Uses a PID loop and requires PID values to be nonzero
-     * 
-     * @param value velocity setpoint for motor to go to. Units are in encoder ticks per 100ms.
+     *
+     * @param value
+     *            velocity setpoint for motor to go to. Units are in encoder ticks per 100ms.
      */
     @Override
     public void setVelocity(double value) {
         set(Velocity, value);
     }
 
-     /**
+    /**
      * Sets the motor to go target velocity using feedforward term as an offset to motor power.
-     * Uses a PID loop and requires PID values to be nonzero
+     * Uses a PID loop and requires PID values to be nonzero.
      * Use this method when PID requires a constant force i.e. gravity compensation on climber or hood
-     * 
-     * @param value velocity setpoint for motor to go to. Units are in encoder ticks per 100ms.
-     * @param feedforward feedforward value to be used as baseline motor power. Value must be between [-1,1]
-     * 
-     * @see <a href="https://docs.ctre-phoenix.com/en/stable/ch16_ClosedLoop.html#arbitrary-feed-forward">CTRE Documentation on feedforward</a>
+     *
+     * @param value
+     *            velocity setpoint for motor to go to. Units are in encoder ticks per 100ms.
+     * @param feedforward
+     *            feedforward value to be used as baseline motor power. Value must be between [-1,1]
+     *
+     * @see <a href=
+     *      "https://docs.ctre-phoenix.com/en/stable/ch16_ClosedLoop.html#arbitrary-feed-forward">CTRE
+     *      Documentation on feedforward</a>
      */
     @Override
     public void setVelocity(double value, double feedforward) {
@@ -88,7 +117,9 @@ public void setToBrakeMode() {
 
     /**
      * Creates a current limit, preventing how much a motor can draw.
-     * @param amp maximum amount of amperage the motor can draw
+     *
+     * @param amp
+     *            maximum amount of amperage the motor can draw
      */
     @Override
     public void setCurrentLimit(double amp) {
@@ -97,9 +128,11 @@ public void setCurrentLimit(double amp) {
 
     /**
      * Creates a current limit, preventing how much a motor can draw.
-     * 
-     * @param amp maximum amount of amperage the motor can draw
-     * @param timeSeconds how long the motor can go above threshold before being limited
+     *
+     * @param amp
+     *            maximum amount of amperage the motor can draw
+     * @param timeSeconds
+     *            how long the motor can go above threshold before being limited
      */
     @Override
     public void setCurrentLimit(double amp, double timeSeconds) {
@@ -107,77 +140,84 @@ public void setCurrentLimit(double amp, double timeSeconds) {
     }
 
     /**
-	 * Get the encoder position. This returns a value in encoder ticks. 2048 encoder ticks result in 1 motor revolution
-	 *
-	 * @return Position of encoder in encoder ticks.
-	 */
+     * Get the encoder position. This returns a value in encoder ticks. 2048 encoder ticks result in 1
+     * motor revolution
+     *
+     * @return Position of encoder in encoder ticks.
+     */
     @Override
     public double getEncoderPosition() {
         return getSelectedSensorPosition();
     }
 
     /**
-	 * Get the encoder velocity. This returns the rate in which the motor is spinning.
+     * Get the encoder velocity. This returns the rate in which the motor is spinning.
      * The value in encoder ticks per 100ms. 2048 encoder ticks result in 1 motor revolution
-	 *
-	 * @return velocity of encoder in encoder ticks.
-	 */
+     *
+     * @return velocity of encoder in encoder ticks.
+     */
     @Override
     public double getEncoderVelocity() {
         return getSelectedSensorVelocity();
     }
 
     /**
-	 * Sets the encoder position to the given value.
-	 *
-	 * @param position Position to set for the selected sensor, value is in encoder ticks.
-	 */
+     * Sets the encoder position to the given value.
+     *
+     * @param position
+     *            Position to set for the selected sensor, value is in encoder ticks.
+     */
     @Override
     public void setEncoderPosition(double position) {
         setSelectedSensorPosition(position);
     }
 
     /**
-	 * Sets the encoder position to 0.
-	 *
-	 */
+     * Sets the encoder position to 0.
+     *
+     */
     @Override
     public void resetEncoder() {
         setEncoderPosition(0);
     }
 
     /**
-	 * Sets the 'P' constant for a PID loop.
-	 * In a loop, this value is multiplied by closed loop error.
-	 *
-	 * @param value Value of the P constant.
-	 */
+     * Sets the 'P' constant for a PID loop.
+     * In a loop, this value is multiplied by closed loop error.
+     *
+     * @param value
+     *            Value of the P constant.
+     */
     @Override
     public void setP(double value) {
         config_kP(0, value);
     }
 
     /**
-	 * Sets the 'D' constant for a PID loop.
-	 *
-	 * This is multiplied by derivative error (slope of error over the last 1ms)
-     * 
-	 * @param value Value of the D constant.
-	 */
+     * Sets the 'D' constant for a PID loop.
+     *
+     * This is multiplied by derivative error (slope of error over the last 1ms)
+     *
+     * @param value
+     *            Value of the D constant.
+     */
     @Override
     public void setD(double value) {
         config_kD(0, value);
     }
 
     /**
-	 * Sets the 'F' constant in PID loop.
+     * Sets the 'F' constant in PID loop.
      * This is a different value than arbitrary feedforward
-	 *
-	 * @see <a href="https://docs.ctre-phoenix.com/en/stable/ch16_ClosedLoop.html?highlight=kF#calculating-velocity-feed-forward-gain-kf">CTRE Documentation on F in PID</a>
-
-	 *
-	 * @param value Value of the F constant.
-	 */
+     *
+     * @see <a href=
+     *      "https://docs.ctre-phoenix.com/en/stable/ch16_ClosedLoop.html?highlight=kF#calculating-velocity-feed-forward-gain-kf">CTRE
+     *      Documentation on F in PID</a>
+     *
+     *
+     * @param value
+     *            Value of the F constant.
+     */
     @Override
     public void setF(double value) {
         config_kF(0, value);
diff --git a/src/main/java/frc/team2412/robot/util/MotorStuff/Motor.java b/src/main/java/frc/team2412/robot/util/MotorStuff/Motor.java
index 716cb128..b600e389 100644
--- a/src/main/java/frc/team2412/robot/util/MotorStuff/Motor.java
+++ b/src/main/java/frc/team2412/robot/util/MotorStuff/Motor.java
@@ -6,6 +6,8 @@ public interface Motor extends MotorController {
 
     public void setPosition(double value);
 
+    public void setPosition(double value, double feedforward);
+
     public void setVelocity(double value);
 
     public void setVelocity(double value, double feedforward);

From a157ca653ce848ff79512e56fcd59fa8a984e0bf Mon Sep 17 00:00:00 2001
From: SumedhP <43325361+SumedhP@users.noreply.github.com>
Date: Fri, 24 Jun 2022 16:11:42 -0700
Subject: [PATCH 11/25] Remy optimized code

---
 .../java/frc/team2412/robot/util/MotorStuff/FalconMotor.java    | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/frc/team2412/robot/util/MotorStuff/FalconMotor.java b/src/main/java/frc/team2412/robot/util/MotorStuff/FalconMotor.java
index be77a27c..065ba046 100644
--- a/src/main/java/frc/team2412/robot/util/MotorStuff/FalconMotor.java
+++ b/src/main/java/frc/team2412/robot/util/MotorStuff/FalconMotor.java
@@ -4,7 +4,7 @@
 import static com.ctre.phoenix.motorcontrol.DemandType.*;
 import static com.ctre.phoenix.motorcontrol.NeutralMode.*;
 
-import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration;
+import com.ctre.phoenix.motorcontrol.*;
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
 
 public class FalconMotor extends WPI_TalonFX implements Motor {

From c143076dfbfc4bac0c447e0b4ca6956b0107dd21 Mon Sep 17 00:00:00 2001
From: SumedhP <43325361+SumedhP@users.noreply.github.com>
Date: Mon, 27 Jun 2022 10:48:24 -0700
Subject: [PATCH 12/25] Remy made code spotless

---
 .../frc/team2412/robot/subsystem/ClimbSubsystem.java | 12 ++++++------
 1 file changed, 6 insertions(+), 6 deletions(-)

diff --git a/src/main/java/frc/team2412/robot/subsystem/ClimbSubsystem.java b/src/main/java/frc/team2412/robot/subsystem/ClimbSubsystem.java
index 5a3400ab..b4ce083e 100644
--- a/src/main/java/frc/team2412/robot/subsystem/ClimbSubsystem.java
+++ b/src/main/java/frc/team2412/robot/subsystem/ClimbSubsystem.java
@@ -90,7 +90,7 @@ public void simInit(PhysicsSim sim) {
      * Stop the dynamic climb arm motor, graciously
      *
      * @param stop
-     *             Whether to stop the motor
+     *            Whether to stop the motor
      */
     @Config(name = "Stop Fixed Motor")
     public void stopArm(boolean stop) {
@@ -121,7 +121,7 @@ public void retractArm() {
      * Set the position to which the motor will graciously follow
      *
      * @param value
-     *              The position to set the motor
+     *            The position to set the motor
      */
     public void setMotor(double value, double feedforward) {
         motor.setPosition(value, feedforward);
@@ -173,7 +173,7 @@ public double climbHeightRemy() {
      * specified as true.
      *
      * @param reset
-     *              whether to reset the encoder
+     *            whether to reset the encoder
      */
     @Config.ToggleButton
     public void resetEncoder(boolean reset) {
@@ -186,11 +186,11 @@ public void resetEncoder(boolean reset) {
      * Configure the motor PID (probably graciously)
      *
      * @param p
-     *          the P value to configure
+     *            the P value to configure
      * @param i
-     *          the I value to configure
+     *            the I value to configure
      * @param d
-     *          the D value to configure
+     *            the D value to configure
      */
     @Config(name = "PID extend")
     private void setPIDExtend(@Config(name = "EXTENSION P", defaultValueNumeric = EXTENSION_P) double p,

From a4d96d5edf7f429d962aafdce3a80dbf1cc9d60c Mon Sep 17 00:00:00 2001
From: SumedhP <43325361+SumedhP@users.noreply.github.com>
Date: Wed, 31 Aug 2022 23:43:18 -0700
Subject: [PATCH 13/25] Undid some motor stuff, better idea incoming soon

---
 .../robot/subsystem/ClimbSubsystem.java       |  47 ++--
 .../robot/util/MotorStuff/FalconMotor.java    | 226 ------------------
 .../team2412/robot/util/MotorStuff/Motor.java |  39 ---
 3 files changed, 28 insertions(+), 284 deletions(-)
 delete mode 100644 src/main/java/frc/team2412/robot/util/MotorStuff/FalconMotor.java
 delete mode 100644 src/main/java/frc/team2412/robot/util/MotorStuff/Motor.java

diff --git a/src/main/java/frc/team2412/robot/subsystem/ClimbSubsystem.java b/src/main/java/frc/team2412/robot/subsystem/ClimbSubsystem.java
index b4ce083e..a9ec611c 100644
--- a/src/main/java/frc/team2412/robot/subsystem/ClimbSubsystem.java
+++ b/src/main/java/frc/team2412/robot/subsystem/ClimbSubsystem.java
@@ -1,16 +1,21 @@
 package frc.team2412.robot.subsystem;
 
-import static frc.team2412.robot.Hardware.*;
 import static frc.team2412.robot.subsystem.ClimbSubsystem.ClimbConstants.*;
+import static frc.team2412.robot.Hardware.*;
 
+import com.ctre.phoenix.motorcontrol.ControlMode;
+import com.ctre.phoenix.motorcontrol.DemandType;
+import com.ctre.phoenix.motorcontrol.NeutralMode;
 import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration;
+import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration;
+import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
 
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.team2412.robot.sim.PhysicsSim;
-import frc.team2412.robot.util.MotorStuff.FalconMotor;
 import io.github.oblarg.oblog.Loggable;
-import io.github.oblarg.oblog.annotations.*;
+import io.github.oblarg.oblog.annotations.Config;
+import io.github.oblarg.oblog.annotations.Log;
 
 public class ClimbSubsystem extends SubsystemBase implements Loggable {
 
@@ -31,8 +36,7 @@ public static class ClimbConstants {
         public static final double RETRACTION_I = 0;
         public static final double RETRACTION_D = 0;
         public static final double RETRACTION_F = 0.18;
-        // This is based on the minimum amount of motor power need to keep climb arm in
-        // place, need to test
+        // This is based on the minimum amount of motor power need to keep climb arm in place, need to test
 
         // Relating to physical climb structure things
         // was previously mid
@@ -55,21 +59,26 @@ public static class ClimbConstants {
     }
 
     @Log.MotorController
-    private final FalconMotor motor;
+    private final WPI_TalonFX motor;
 
     // For use in the simulationPeriodic to get encoder position
     private double lastUpdatedTime = Timer.getFPGATimestamp();
 
     public ClimbSubsystem() {
         setName("ClimbSubsystem");
-        motor = new FalconMotor(CLIMB_FIXED_MOTOR);
-
-        motor.setToBrakeMode();
-        motor.setCurrentLimit(40, 15);
-
-        motor.setP(EXTENSION_P);
-        motor.setD(EXTENSION_D);
-
+        motor = new WPI_TalonFX(CLIMB_FIXED_MOTOR);
+
+        // Configure motor soft limits, current limits and peak outputs
+        TalonFXConfiguration motorConfig = new TalonFXConfiguration();
+        motorConfig.forwardSoftLimitEnable = false;
+        motorConfig.reverseSoftLimitEnable = false;
+        motorConfig.forwardSoftLimitThreshold = MAX_ENCODER_TICKS;
+        motorConfig.reverseSoftLimitThreshold = MIN_ENCODER_TICKS;
+        motorConfig.supplyCurrLimit = MOTOR_CURRENT_LIMIT;
+        motor.configAllSettings(motorConfig);
+        motor.setNeutralMode(NeutralMode.Brake);
+
+        setPIDExtend(EXTENSION_P, EXTENSION_I, EXTENSION_D);
         setPIDRetract(RETRACTION_P, RETRACTION_I, RETRACTION_D);
     }
 
@@ -123,8 +132,8 @@ public void retractArm() {
      * @param value
      *            The position to set the motor
      */
-    public void setMotor(double value, double feedforward) {
-        motor.setPosition(value, feedforward);
+    public void setMotor(double value, double feedForward) {
+        motor.set(ControlMode.Position, value, DemandType.ArbitraryFeedForward, feedForward);
     }
 
     public void setMotorSpeed(double speed) {
@@ -153,7 +162,7 @@ public void simulationPeriodic() {
      */
     @Log
     public double encoderPosition() {
-        return motor.getEncoderPosition();
+        return motor.getSelectedSensorPosition();
     }
 
     /**
@@ -178,7 +187,7 @@ public double climbHeightRemy() {
     @Config.ToggleButton
     public void resetEncoder(boolean reset) {
         if (reset) {
-            motor.resetEncoder();
+            motor.setSelectedSensorPosition(0);
         }
     }
 
@@ -233,4 +242,4 @@ public boolean isHittingLimitSwitch() {
         return true;
     }
 
-}
+}
\ No newline at end of file
diff --git a/src/main/java/frc/team2412/robot/util/MotorStuff/FalconMotor.java b/src/main/java/frc/team2412/robot/util/MotorStuff/FalconMotor.java
deleted file mode 100644
index 065ba046..00000000
--- a/src/main/java/frc/team2412/robot/util/MotorStuff/FalconMotor.java
+++ /dev/null
@@ -1,226 +0,0 @@
-package frc.team2412.robot.util.MotorStuff;
-
-import static com.ctre.phoenix.motorcontrol.ControlMode.*;
-import static com.ctre.phoenix.motorcontrol.DemandType.*;
-import static com.ctre.phoenix.motorcontrol.NeutralMode.*;
-
-import com.ctre.phoenix.motorcontrol.*;
-import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
-
-public class FalconMotor extends WPI_TalonFX implements Motor {
-
-    /**
-     * Constructor for motor
-     *
-     * @param deviceNumber
-     *            device ID of motor controller (this value is assigned in pheonix tuner)
-     */
-    public FalconMotor(int deviceNumber) {
-        super(deviceNumber);
-    }
-
-    /**
-     * Constructor for motor. Use in case the motor is on a different CAN network.
-     * This is used primarily for any devices on a CANivore network
-     *
-     * @param deviceNumber
-     *            device ID of motor controller (this value is assigned in pheonix tuner)
-     * @param canbus
-     *            Name of the CANbus (Canivore name can be found in pheonix tuner)
-     *
-     */
-    public FalconMotor(int deviceNumber, String canbus) {
-        super(deviceNumber, canbus);
-    }
-
-    /**
-     * Sets the motor to the target position.
-     * Uses a PID loop and requires PID values to be nonzero
-     *
-     * @param value
-     *            position setpoint for motor to go to. Units are in encoder ticks.
-     */
-    @Override
-    public void setPosition(double value) {
-        set(Position, value);
-    }
-
-    /**
-     * Sets the motor to the target position using feedforward term as an offset to motor power.
-     * Uses a PID loop and requires PID values to be nonzero.
-     * Use this method when PID requires a constant force i.e. gravity compensation on climber or hood
-     *
-     * @param value
-     *            position setpoint for motor to go to. Units are in encoder ticks.
-     * @param feedforward
-     *            feedforward value to be used as baseline motor power. Value must be between [-1,1]
-     *
-     * @see <a href=
-     *      "https://docs.ctre-phoenix.com/en/stable/ch16_ClosedLoop.html#arbitrary-feed-forward">CTRE
-     *      Documentation on feedforward</a>
-     */
-    @Override
-    public void setPosition(double value, double feedforward) {
-        set(Position, value, ArbitraryFeedForward, feedforward);
-    }
-
-    /**
-     * Sets the motor to go target velocity.
-     * Uses a PID loop and requires PID values to be nonzero
-     *
-     * @param value
-     *            velocity setpoint for motor to go to. Units are in encoder ticks per 100ms.
-     */
-    @Override
-    public void setVelocity(double value) {
-        set(Velocity, value);
-    }
-
-    /**
-     * Sets the motor to go target velocity using feedforward term as an offset to motor power.
-     * Uses a PID loop and requires PID values to be nonzero.
-     * Use this method when PID requires a constant force i.e. gravity compensation on climber or hood
-     *
-     * @param value
-     *            velocity setpoint for motor to go to. Units are in encoder ticks per 100ms.
-     * @param feedforward
-     *            feedforward value to be used as baseline motor power. Value must be between [-1,1]
-     *
-     * @see <a href=
-     *      "https://docs.ctre-phoenix.com/en/stable/ch16_ClosedLoop.html#arbitrary-feed-forward">CTRE
-     *      Documentation on feedforward</a>
-     */
-    @Override
-    public void setVelocity(double value, double feedforward) {
-        set(Velocity, value, ArbitraryFeedForward, feedforward);
-    }
-
-    /**
-     * Sets motor to coast, allowing motor to freely spin when acted upon other forces.
-     * Motor will provide no resistance to rotation.
-     * If wanting motor to stay in place, use brake mode
-     */
-    @Override
-    public void setToCoastMode() {
-        setNeutralMode(Coast);
-    }
-
-    /**
-     * Sets motor to brake, causing motor to resist outside forces.
-     * Using this will cause the motor to apply electrical resistance to rotation.
-     * If wanting motor to spin freely, use coast mode
-     */
-    @Override
-    public void setToBrakeMode() {
-        setNeutralMode(Brake);
-    }
-
-    /**
-     * Creates a current limit, preventing how much a motor can draw.
-     *
-     * @param amp
-     *            maximum amount of amperage the motor can draw
-     */
-    @Override
-    public void setCurrentLimit(double amp) {
-        configSupplyCurrentLimit(new SupplyCurrentLimitConfiguration(true, amp, 0, 0));
-    }
-
-    /**
-     * Creates a current limit, preventing how much a motor can draw.
-     *
-     * @param amp
-     *            maximum amount of amperage the motor can draw
-     * @param timeSeconds
-     *            how long the motor can go above threshold before being limited
-     */
-    @Override
-    public void setCurrentLimit(double amp, double timeSeconds) {
-        configSupplyCurrentLimit(new SupplyCurrentLimitConfiguration(true, amp, amp, timeSeconds));
-    }
-
-    /**
-     * Get the encoder position. This returns a value in encoder ticks. 2048 encoder ticks result in 1
-     * motor revolution
-     *
-     * @return Position of encoder in encoder ticks.
-     */
-    @Override
-    public double getEncoderPosition() {
-        return getSelectedSensorPosition();
-    }
-
-    /**
-     * Get the encoder velocity. This returns the rate in which the motor is spinning.
-     * The value in encoder ticks per 100ms. 2048 encoder ticks result in 1 motor revolution
-     *
-     * @return velocity of encoder in encoder ticks.
-     */
-    @Override
-    public double getEncoderVelocity() {
-        return getSelectedSensorVelocity();
-    }
-
-    /**
-     * Sets the encoder position to the given value.
-     *
-     * @param position
-     *            Position to set for the selected sensor, value is in encoder ticks.
-     */
-    @Override
-    public void setEncoderPosition(double position) {
-        setSelectedSensorPosition(position);
-    }
-
-    /**
-     * Sets the encoder position to 0.
-     *
-     */
-    @Override
-    public void resetEncoder() {
-        setEncoderPosition(0);
-    }
-
-    /**
-     * Sets the 'P' constant for a PID loop.
-     * In a loop, this value is multiplied by closed loop error.
-     *
-     * @param value
-     *            Value of the P constant.
-     */
-    @Override
-    public void setP(double value) {
-        config_kP(0, value);
-    }
-
-    /**
-     * Sets the 'D' constant for a PID loop.
-     *
-     * This is multiplied by derivative error (slope of error over the last 1ms)
-     *
-     * @param value
-     *            Value of the D constant.
-     */
-    @Override
-    public void setD(double value) {
-        config_kD(0, value);
-    }
-
-    /**
-     * Sets the 'F' constant in PID loop.
-     * This is a different value than arbitrary feedforward
-     *
-     * @see <a href=
-     *      "https://docs.ctre-phoenix.com/en/stable/ch16_ClosedLoop.html?highlight=kF#calculating-velocity-feed-forward-gain-kf">CTRE
-     *      Documentation on F in PID</a>
-     *
-     *
-     * @param value
-     *            Value of the F constant.
-     */
-    @Override
-    public void setF(double value) {
-        config_kF(0, value);
-    }
-
-}
diff --git a/src/main/java/frc/team2412/robot/util/MotorStuff/Motor.java b/src/main/java/frc/team2412/robot/util/MotorStuff/Motor.java
deleted file mode 100644
index b600e389..00000000
--- a/src/main/java/frc/team2412/robot/util/MotorStuff/Motor.java
+++ /dev/null
@@ -1,39 +0,0 @@
-package frc.team2412.robot.util.MotorStuff;
-
-import edu.wpi.first.wpilibj.motorcontrol.MotorController;
-
-public interface Motor extends MotorController {
-
-    public void setPosition(double value);
-
-    public void setPosition(double value, double feedforward);
-
-    public void setVelocity(double value);
-
-    public void setVelocity(double value, double feedforward);
-
-    public void setVoltage(double voltage);
-
-    public void setToCoastMode();
-
-    public void setToBrakeMode();
-
-    public void setCurrentLimit(double amp);
-
-    public void setCurrentLimit(double amp, double timeMs);
-
-    public double getEncoderPosition();
-
-    public double getEncoderVelocity();
-
-    public void setEncoderPosition(double position);
-
-    public void resetEncoder();
-
-    public void setP(double value);
-
-    public void setD(double value);
-
-    public void setF(double value);
-
-}

From 299c2c58a38ab5f5e43a14e42184908088d96156 Mon Sep 17 00:00:00 2001
From: SumedhP <43325361+SumedhP@users.noreply.github.com>
Date: Wed, 31 Aug 2022 23:44:24 -0700
Subject: [PATCH 14/25] bad

---
 src/main/java/frc/team2412/robot/subsystem/ClimbSubsystem.java | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/frc/team2412/robot/subsystem/ClimbSubsystem.java b/src/main/java/frc/team2412/robot/subsystem/ClimbSubsystem.java
index a9ec611c..9eff2ea2 100644
--- a/src/main/java/frc/team2412/robot/subsystem/ClimbSubsystem.java
+++ b/src/main/java/frc/team2412/robot/subsystem/ClimbSubsystem.java
@@ -242,4 +242,4 @@ public boolean isHittingLimitSwitch() {
         return true;
     }
 
-}
\ No newline at end of file
+}

From bed45a4e0630b47b6e7687540c85f1186b594072 Mon Sep 17 00:00:00 2001
From: SumedhP <43325361+SumedhP@users.noreply.github.com>
Date: Wed, 31 Aug 2022 23:44:32 -0700
Subject: [PATCH 15/25] Revert "Undid some motor stuff, better idea incoming
 soon"

This reverts commit a4d96d5edf7f429d962aafdce3a80dbf1cc9d60c.
---
 .../robot/subsystem/ClimbSubsystem.java       |  45 ++--
 .../robot/util/MotorStuff/FalconMotor.java    | 226 ++++++++++++++++++
 .../team2412/robot/util/MotorStuff/Motor.java |  39 +++
 3 files changed, 283 insertions(+), 27 deletions(-)
 create mode 100644 src/main/java/frc/team2412/robot/util/MotorStuff/FalconMotor.java
 create mode 100644 src/main/java/frc/team2412/robot/util/MotorStuff/Motor.java

diff --git a/src/main/java/frc/team2412/robot/subsystem/ClimbSubsystem.java b/src/main/java/frc/team2412/robot/subsystem/ClimbSubsystem.java
index 9eff2ea2..b4ce083e 100644
--- a/src/main/java/frc/team2412/robot/subsystem/ClimbSubsystem.java
+++ b/src/main/java/frc/team2412/robot/subsystem/ClimbSubsystem.java
@@ -1,21 +1,16 @@
 package frc.team2412.robot.subsystem;
 
-import static frc.team2412.robot.subsystem.ClimbSubsystem.ClimbConstants.*;
 import static frc.team2412.robot.Hardware.*;
+import static frc.team2412.robot.subsystem.ClimbSubsystem.ClimbConstants.*;
 
-import com.ctre.phoenix.motorcontrol.ControlMode;
-import com.ctre.phoenix.motorcontrol.DemandType;
-import com.ctre.phoenix.motorcontrol.NeutralMode;
 import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration;
-import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration;
-import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
 
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.team2412.robot.sim.PhysicsSim;
+import frc.team2412.robot.util.MotorStuff.FalconMotor;
 import io.github.oblarg.oblog.Loggable;
-import io.github.oblarg.oblog.annotations.Config;
-import io.github.oblarg.oblog.annotations.Log;
+import io.github.oblarg.oblog.annotations.*;
 
 public class ClimbSubsystem extends SubsystemBase implements Loggable {
 
@@ -36,7 +31,8 @@ public static class ClimbConstants {
         public static final double RETRACTION_I = 0;
         public static final double RETRACTION_D = 0;
         public static final double RETRACTION_F = 0.18;
-        // This is based on the minimum amount of motor power need to keep climb arm in place, need to test
+        // This is based on the minimum amount of motor power need to keep climb arm in
+        // place, need to test
 
         // Relating to physical climb structure things
         // was previously mid
@@ -59,26 +55,21 @@ public static class ClimbConstants {
     }
 
     @Log.MotorController
-    private final WPI_TalonFX motor;
+    private final FalconMotor motor;
 
     // For use in the simulationPeriodic to get encoder position
     private double lastUpdatedTime = Timer.getFPGATimestamp();
 
     public ClimbSubsystem() {
         setName("ClimbSubsystem");
-        motor = new WPI_TalonFX(CLIMB_FIXED_MOTOR);
-
-        // Configure motor soft limits, current limits and peak outputs
-        TalonFXConfiguration motorConfig = new TalonFXConfiguration();
-        motorConfig.forwardSoftLimitEnable = false;
-        motorConfig.reverseSoftLimitEnable = false;
-        motorConfig.forwardSoftLimitThreshold = MAX_ENCODER_TICKS;
-        motorConfig.reverseSoftLimitThreshold = MIN_ENCODER_TICKS;
-        motorConfig.supplyCurrLimit = MOTOR_CURRENT_LIMIT;
-        motor.configAllSettings(motorConfig);
-        motor.setNeutralMode(NeutralMode.Brake);
-
-        setPIDExtend(EXTENSION_P, EXTENSION_I, EXTENSION_D);
+        motor = new FalconMotor(CLIMB_FIXED_MOTOR);
+
+        motor.setToBrakeMode();
+        motor.setCurrentLimit(40, 15);
+
+        motor.setP(EXTENSION_P);
+        motor.setD(EXTENSION_D);
+
         setPIDRetract(RETRACTION_P, RETRACTION_I, RETRACTION_D);
     }
 
@@ -132,8 +123,8 @@ public void retractArm() {
      * @param value
      *            The position to set the motor
      */
-    public void setMotor(double value, double feedForward) {
-        motor.set(ControlMode.Position, value, DemandType.ArbitraryFeedForward, feedForward);
+    public void setMotor(double value, double feedforward) {
+        motor.setPosition(value, feedforward);
     }
 
     public void setMotorSpeed(double speed) {
@@ -162,7 +153,7 @@ public void simulationPeriodic() {
      */
     @Log
     public double encoderPosition() {
-        return motor.getSelectedSensorPosition();
+        return motor.getEncoderPosition();
     }
 
     /**
@@ -187,7 +178,7 @@ public double climbHeightRemy() {
     @Config.ToggleButton
     public void resetEncoder(boolean reset) {
         if (reset) {
-            motor.setSelectedSensorPosition(0);
+            motor.resetEncoder();
         }
     }
 
diff --git a/src/main/java/frc/team2412/robot/util/MotorStuff/FalconMotor.java b/src/main/java/frc/team2412/robot/util/MotorStuff/FalconMotor.java
new file mode 100644
index 00000000..065ba046
--- /dev/null
+++ b/src/main/java/frc/team2412/robot/util/MotorStuff/FalconMotor.java
@@ -0,0 +1,226 @@
+package frc.team2412.robot.util.MotorStuff;
+
+import static com.ctre.phoenix.motorcontrol.ControlMode.*;
+import static com.ctre.phoenix.motorcontrol.DemandType.*;
+import static com.ctre.phoenix.motorcontrol.NeutralMode.*;
+
+import com.ctre.phoenix.motorcontrol.*;
+import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
+
+public class FalconMotor extends WPI_TalonFX implements Motor {
+
+    /**
+     * Constructor for motor
+     *
+     * @param deviceNumber
+     *            device ID of motor controller (this value is assigned in pheonix tuner)
+     */
+    public FalconMotor(int deviceNumber) {
+        super(deviceNumber);
+    }
+
+    /**
+     * Constructor for motor. Use in case the motor is on a different CAN network.
+     * This is used primarily for any devices on a CANivore network
+     *
+     * @param deviceNumber
+     *            device ID of motor controller (this value is assigned in pheonix tuner)
+     * @param canbus
+     *            Name of the CANbus (Canivore name can be found in pheonix tuner)
+     *
+     */
+    public FalconMotor(int deviceNumber, String canbus) {
+        super(deviceNumber, canbus);
+    }
+
+    /**
+     * Sets the motor to the target position.
+     * Uses a PID loop and requires PID values to be nonzero
+     *
+     * @param value
+     *            position setpoint for motor to go to. Units are in encoder ticks.
+     */
+    @Override
+    public void setPosition(double value) {
+        set(Position, value);
+    }
+
+    /**
+     * Sets the motor to the target position using feedforward term as an offset to motor power.
+     * Uses a PID loop and requires PID values to be nonzero.
+     * Use this method when PID requires a constant force i.e. gravity compensation on climber or hood
+     *
+     * @param value
+     *            position setpoint for motor to go to. Units are in encoder ticks.
+     * @param feedforward
+     *            feedforward value to be used as baseline motor power. Value must be between [-1,1]
+     *
+     * @see <a href=
+     *      "https://docs.ctre-phoenix.com/en/stable/ch16_ClosedLoop.html#arbitrary-feed-forward">CTRE
+     *      Documentation on feedforward</a>
+     */
+    @Override
+    public void setPosition(double value, double feedforward) {
+        set(Position, value, ArbitraryFeedForward, feedforward);
+    }
+
+    /**
+     * Sets the motor to go target velocity.
+     * Uses a PID loop and requires PID values to be nonzero
+     *
+     * @param value
+     *            velocity setpoint for motor to go to. Units are in encoder ticks per 100ms.
+     */
+    @Override
+    public void setVelocity(double value) {
+        set(Velocity, value);
+    }
+
+    /**
+     * Sets the motor to go target velocity using feedforward term as an offset to motor power.
+     * Uses a PID loop and requires PID values to be nonzero.
+     * Use this method when PID requires a constant force i.e. gravity compensation on climber or hood
+     *
+     * @param value
+     *            velocity setpoint for motor to go to. Units are in encoder ticks per 100ms.
+     * @param feedforward
+     *            feedforward value to be used as baseline motor power. Value must be between [-1,1]
+     *
+     * @see <a href=
+     *      "https://docs.ctre-phoenix.com/en/stable/ch16_ClosedLoop.html#arbitrary-feed-forward">CTRE
+     *      Documentation on feedforward</a>
+     */
+    @Override
+    public void setVelocity(double value, double feedforward) {
+        set(Velocity, value, ArbitraryFeedForward, feedforward);
+    }
+
+    /**
+     * Sets motor to coast, allowing motor to freely spin when acted upon other forces.
+     * Motor will provide no resistance to rotation.
+     * If wanting motor to stay in place, use brake mode
+     */
+    @Override
+    public void setToCoastMode() {
+        setNeutralMode(Coast);
+    }
+
+    /**
+     * Sets motor to brake, causing motor to resist outside forces.
+     * Using this will cause the motor to apply electrical resistance to rotation.
+     * If wanting motor to spin freely, use coast mode
+     */
+    @Override
+    public void setToBrakeMode() {
+        setNeutralMode(Brake);
+    }
+
+    /**
+     * Creates a current limit, preventing how much a motor can draw.
+     *
+     * @param amp
+     *            maximum amount of amperage the motor can draw
+     */
+    @Override
+    public void setCurrentLimit(double amp) {
+        configSupplyCurrentLimit(new SupplyCurrentLimitConfiguration(true, amp, 0, 0));
+    }
+
+    /**
+     * Creates a current limit, preventing how much a motor can draw.
+     *
+     * @param amp
+     *            maximum amount of amperage the motor can draw
+     * @param timeSeconds
+     *            how long the motor can go above threshold before being limited
+     */
+    @Override
+    public void setCurrentLimit(double amp, double timeSeconds) {
+        configSupplyCurrentLimit(new SupplyCurrentLimitConfiguration(true, amp, amp, timeSeconds));
+    }
+
+    /**
+     * Get the encoder position. This returns a value in encoder ticks. 2048 encoder ticks result in 1
+     * motor revolution
+     *
+     * @return Position of encoder in encoder ticks.
+     */
+    @Override
+    public double getEncoderPosition() {
+        return getSelectedSensorPosition();
+    }
+
+    /**
+     * Get the encoder velocity. This returns the rate in which the motor is spinning.
+     * The value in encoder ticks per 100ms. 2048 encoder ticks result in 1 motor revolution
+     *
+     * @return velocity of encoder in encoder ticks.
+     */
+    @Override
+    public double getEncoderVelocity() {
+        return getSelectedSensorVelocity();
+    }
+
+    /**
+     * Sets the encoder position to the given value.
+     *
+     * @param position
+     *            Position to set for the selected sensor, value is in encoder ticks.
+     */
+    @Override
+    public void setEncoderPosition(double position) {
+        setSelectedSensorPosition(position);
+    }
+
+    /**
+     * Sets the encoder position to 0.
+     *
+     */
+    @Override
+    public void resetEncoder() {
+        setEncoderPosition(0);
+    }
+
+    /**
+     * Sets the 'P' constant for a PID loop.
+     * In a loop, this value is multiplied by closed loop error.
+     *
+     * @param value
+     *            Value of the P constant.
+     */
+    @Override
+    public void setP(double value) {
+        config_kP(0, value);
+    }
+
+    /**
+     * Sets the 'D' constant for a PID loop.
+     *
+     * This is multiplied by derivative error (slope of error over the last 1ms)
+     *
+     * @param value
+     *            Value of the D constant.
+     */
+    @Override
+    public void setD(double value) {
+        config_kD(0, value);
+    }
+
+    /**
+     * Sets the 'F' constant in PID loop.
+     * This is a different value than arbitrary feedforward
+     *
+     * @see <a href=
+     *      "https://docs.ctre-phoenix.com/en/stable/ch16_ClosedLoop.html?highlight=kF#calculating-velocity-feed-forward-gain-kf">CTRE
+     *      Documentation on F in PID</a>
+     *
+     *
+     * @param value
+     *            Value of the F constant.
+     */
+    @Override
+    public void setF(double value) {
+        config_kF(0, value);
+    }
+
+}
diff --git a/src/main/java/frc/team2412/robot/util/MotorStuff/Motor.java b/src/main/java/frc/team2412/robot/util/MotorStuff/Motor.java
new file mode 100644
index 00000000..b600e389
--- /dev/null
+++ b/src/main/java/frc/team2412/robot/util/MotorStuff/Motor.java
@@ -0,0 +1,39 @@
+package frc.team2412.robot.util.MotorStuff;
+
+import edu.wpi.first.wpilibj.motorcontrol.MotorController;
+
+public interface Motor extends MotorController {
+
+    public void setPosition(double value);
+
+    public void setPosition(double value, double feedforward);
+
+    public void setVelocity(double value);
+
+    public void setVelocity(double value, double feedforward);
+
+    public void setVoltage(double voltage);
+
+    public void setToCoastMode();
+
+    public void setToBrakeMode();
+
+    public void setCurrentLimit(double amp);
+
+    public void setCurrentLimit(double amp, double timeMs);
+
+    public double getEncoderPosition();
+
+    public double getEncoderVelocity();
+
+    public void setEncoderPosition(double position);
+
+    public void resetEncoder();
+
+    public void setP(double value);
+
+    public void setD(double value);
+
+    public void setF(double value);
+
+}

From 6c82cda1971fac8d9a2be2f67d47e92867baf5e5 Mon Sep 17 00:00:00 2001
From: SumedhP <43325361+SumedhP@users.noreply.github.com>
Date: Wed, 31 Aug 2022 23:44:43 -0700
Subject: [PATCH 16/25] Remy Undid some motor stuff, better idea incoming soon

---
 .../robot/subsystem/ClimbSubsystem.java       |  45 ++--
 .../robot/util/MotorStuff/FalconMotor.java    | 226 ------------------
 .../team2412/robot/util/MotorStuff/Motor.java |  39 ---
 3 files changed, 27 insertions(+), 283 deletions(-)
 delete mode 100644 src/main/java/frc/team2412/robot/util/MotorStuff/FalconMotor.java
 delete mode 100644 src/main/java/frc/team2412/robot/util/MotorStuff/Motor.java

diff --git a/src/main/java/frc/team2412/robot/subsystem/ClimbSubsystem.java b/src/main/java/frc/team2412/robot/subsystem/ClimbSubsystem.java
index b4ce083e..9eff2ea2 100644
--- a/src/main/java/frc/team2412/robot/subsystem/ClimbSubsystem.java
+++ b/src/main/java/frc/team2412/robot/subsystem/ClimbSubsystem.java
@@ -1,16 +1,21 @@
 package frc.team2412.robot.subsystem;
 
-import static frc.team2412.robot.Hardware.*;
 import static frc.team2412.robot.subsystem.ClimbSubsystem.ClimbConstants.*;
+import static frc.team2412.robot.Hardware.*;
 
+import com.ctre.phoenix.motorcontrol.ControlMode;
+import com.ctre.phoenix.motorcontrol.DemandType;
+import com.ctre.phoenix.motorcontrol.NeutralMode;
 import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration;
+import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration;
+import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
 
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.team2412.robot.sim.PhysicsSim;
-import frc.team2412.robot.util.MotorStuff.FalconMotor;
 import io.github.oblarg.oblog.Loggable;
-import io.github.oblarg.oblog.annotations.*;
+import io.github.oblarg.oblog.annotations.Config;
+import io.github.oblarg.oblog.annotations.Log;
 
 public class ClimbSubsystem extends SubsystemBase implements Loggable {
 
@@ -31,8 +36,7 @@ public static class ClimbConstants {
         public static final double RETRACTION_I = 0;
         public static final double RETRACTION_D = 0;
         public static final double RETRACTION_F = 0.18;
-        // This is based on the minimum amount of motor power need to keep climb arm in
-        // place, need to test
+        // This is based on the minimum amount of motor power need to keep climb arm in place, need to test
 
         // Relating to physical climb structure things
         // was previously mid
@@ -55,21 +59,26 @@ public static class ClimbConstants {
     }
 
     @Log.MotorController
-    private final FalconMotor motor;
+    private final WPI_TalonFX motor;
 
     // For use in the simulationPeriodic to get encoder position
     private double lastUpdatedTime = Timer.getFPGATimestamp();
 
     public ClimbSubsystem() {
         setName("ClimbSubsystem");
-        motor = new FalconMotor(CLIMB_FIXED_MOTOR);
-
-        motor.setToBrakeMode();
-        motor.setCurrentLimit(40, 15);
-
-        motor.setP(EXTENSION_P);
-        motor.setD(EXTENSION_D);
-
+        motor = new WPI_TalonFX(CLIMB_FIXED_MOTOR);
+
+        // Configure motor soft limits, current limits and peak outputs
+        TalonFXConfiguration motorConfig = new TalonFXConfiguration();
+        motorConfig.forwardSoftLimitEnable = false;
+        motorConfig.reverseSoftLimitEnable = false;
+        motorConfig.forwardSoftLimitThreshold = MAX_ENCODER_TICKS;
+        motorConfig.reverseSoftLimitThreshold = MIN_ENCODER_TICKS;
+        motorConfig.supplyCurrLimit = MOTOR_CURRENT_LIMIT;
+        motor.configAllSettings(motorConfig);
+        motor.setNeutralMode(NeutralMode.Brake);
+
+        setPIDExtend(EXTENSION_P, EXTENSION_I, EXTENSION_D);
         setPIDRetract(RETRACTION_P, RETRACTION_I, RETRACTION_D);
     }
 
@@ -123,8 +132,8 @@ public void retractArm() {
      * @param value
      *            The position to set the motor
      */
-    public void setMotor(double value, double feedforward) {
-        motor.setPosition(value, feedforward);
+    public void setMotor(double value, double feedForward) {
+        motor.set(ControlMode.Position, value, DemandType.ArbitraryFeedForward, feedForward);
     }
 
     public void setMotorSpeed(double speed) {
@@ -153,7 +162,7 @@ public void simulationPeriodic() {
      */
     @Log
     public double encoderPosition() {
-        return motor.getEncoderPosition();
+        return motor.getSelectedSensorPosition();
     }
 
     /**
@@ -178,7 +187,7 @@ public double climbHeightRemy() {
     @Config.ToggleButton
     public void resetEncoder(boolean reset) {
         if (reset) {
-            motor.resetEncoder();
+            motor.setSelectedSensorPosition(0);
         }
     }
 
diff --git a/src/main/java/frc/team2412/robot/util/MotorStuff/FalconMotor.java b/src/main/java/frc/team2412/robot/util/MotorStuff/FalconMotor.java
deleted file mode 100644
index 065ba046..00000000
--- a/src/main/java/frc/team2412/robot/util/MotorStuff/FalconMotor.java
+++ /dev/null
@@ -1,226 +0,0 @@
-package frc.team2412.robot.util.MotorStuff;
-
-import static com.ctre.phoenix.motorcontrol.ControlMode.*;
-import static com.ctre.phoenix.motorcontrol.DemandType.*;
-import static com.ctre.phoenix.motorcontrol.NeutralMode.*;
-
-import com.ctre.phoenix.motorcontrol.*;
-import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
-
-public class FalconMotor extends WPI_TalonFX implements Motor {
-
-    /**
-     * Constructor for motor
-     *
-     * @param deviceNumber
-     *            device ID of motor controller (this value is assigned in pheonix tuner)
-     */
-    public FalconMotor(int deviceNumber) {
-        super(deviceNumber);
-    }
-
-    /**
-     * Constructor for motor. Use in case the motor is on a different CAN network.
-     * This is used primarily for any devices on a CANivore network
-     *
-     * @param deviceNumber
-     *            device ID of motor controller (this value is assigned in pheonix tuner)
-     * @param canbus
-     *            Name of the CANbus (Canivore name can be found in pheonix tuner)
-     *
-     */
-    public FalconMotor(int deviceNumber, String canbus) {
-        super(deviceNumber, canbus);
-    }
-
-    /**
-     * Sets the motor to the target position.
-     * Uses a PID loop and requires PID values to be nonzero
-     *
-     * @param value
-     *            position setpoint for motor to go to. Units are in encoder ticks.
-     */
-    @Override
-    public void setPosition(double value) {
-        set(Position, value);
-    }
-
-    /**
-     * Sets the motor to the target position using feedforward term as an offset to motor power.
-     * Uses a PID loop and requires PID values to be nonzero.
-     * Use this method when PID requires a constant force i.e. gravity compensation on climber or hood
-     *
-     * @param value
-     *            position setpoint for motor to go to. Units are in encoder ticks.
-     * @param feedforward
-     *            feedforward value to be used as baseline motor power. Value must be between [-1,1]
-     *
-     * @see <a href=
-     *      "https://docs.ctre-phoenix.com/en/stable/ch16_ClosedLoop.html#arbitrary-feed-forward">CTRE
-     *      Documentation on feedforward</a>
-     */
-    @Override
-    public void setPosition(double value, double feedforward) {
-        set(Position, value, ArbitraryFeedForward, feedforward);
-    }
-
-    /**
-     * Sets the motor to go target velocity.
-     * Uses a PID loop and requires PID values to be nonzero
-     *
-     * @param value
-     *            velocity setpoint for motor to go to. Units are in encoder ticks per 100ms.
-     */
-    @Override
-    public void setVelocity(double value) {
-        set(Velocity, value);
-    }
-
-    /**
-     * Sets the motor to go target velocity using feedforward term as an offset to motor power.
-     * Uses a PID loop and requires PID values to be nonzero.
-     * Use this method when PID requires a constant force i.e. gravity compensation on climber or hood
-     *
-     * @param value
-     *            velocity setpoint for motor to go to. Units are in encoder ticks per 100ms.
-     * @param feedforward
-     *            feedforward value to be used as baseline motor power. Value must be between [-1,1]
-     *
-     * @see <a href=
-     *      "https://docs.ctre-phoenix.com/en/stable/ch16_ClosedLoop.html#arbitrary-feed-forward">CTRE
-     *      Documentation on feedforward</a>
-     */
-    @Override
-    public void setVelocity(double value, double feedforward) {
-        set(Velocity, value, ArbitraryFeedForward, feedforward);
-    }
-
-    /**
-     * Sets motor to coast, allowing motor to freely spin when acted upon other forces.
-     * Motor will provide no resistance to rotation.
-     * If wanting motor to stay in place, use brake mode
-     */
-    @Override
-    public void setToCoastMode() {
-        setNeutralMode(Coast);
-    }
-
-    /**
-     * Sets motor to brake, causing motor to resist outside forces.
-     * Using this will cause the motor to apply electrical resistance to rotation.
-     * If wanting motor to spin freely, use coast mode
-     */
-    @Override
-    public void setToBrakeMode() {
-        setNeutralMode(Brake);
-    }
-
-    /**
-     * Creates a current limit, preventing how much a motor can draw.
-     *
-     * @param amp
-     *            maximum amount of amperage the motor can draw
-     */
-    @Override
-    public void setCurrentLimit(double amp) {
-        configSupplyCurrentLimit(new SupplyCurrentLimitConfiguration(true, amp, 0, 0));
-    }
-
-    /**
-     * Creates a current limit, preventing how much a motor can draw.
-     *
-     * @param amp
-     *            maximum amount of amperage the motor can draw
-     * @param timeSeconds
-     *            how long the motor can go above threshold before being limited
-     */
-    @Override
-    public void setCurrentLimit(double amp, double timeSeconds) {
-        configSupplyCurrentLimit(new SupplyCurrentLimitConfiguration(true, amp, amp, timeSeconds));
-    }
-
-    /**
-     * Get the encoder position. This returns a value in encoder ticks. 2048 encoder ticks result in 1
-     * motor revolution
-     *
-     * @return Position of encoder in encoder ticks.
-     */
-    @Override
-    public double getEncoderPosition() {
-        return getSelectedSensorPosition();
-    }
-
-    /**
-     * Get the encoder velocity. This returns the rate in which the motor is spinning.
-     * The value in encoder ticks per 100ms. 2048 encoder ticks result in 1 motor revolution
-     *
-     * @return velocity of encoder in encoder ticks.
-     */
-    @Override
-    public double getEncoderVelocity() {
-        return getSelectedSensorVelocity();
-    }
-
-    /**
-     * Sets the encoder position to the given value.
-     *
-     * @param position
-     *            Position to set for the selected sensor, value is in encoder ticks.
-     */
-    @Override
-    public void setEncoderPosition(double position) {
-        setSelectedSensorPosition(position);
-    }
-
-    /**
-     * Sets the encoder position to 0.
-     *
-     */
-    @Override
-    public void resetEncoder() {
-        setEncoderPosition(0);
-    }
-
-    /**
-     * Sets the 'P' constant for a PID loop.
-     * In a loop, this value is multiplied by closed loop error.
-     *
-     * @param value
-     *            Value of the P constant.
-     */
-    @Override
-    public void setP(double value) {
-        config_kP(0, value);
-    }
-
-    /**
-     * Sets the 'D' constant for a PID loop.
-     *
-     * This is multiplied by derivative error (slope of error over the last 1ms)
-     *
-     * @param value
-     *            Value of the D constant.
-     */
-    @Override
-    public void setD(double value) {
-        config_kD(0, value);
-    }
-
-    /**
-     * Sets the 'F' constant in PID loop.
-     * This is a different value than arbitrary feedforward
-     *
-     * @see <a href=
-     *      "https://docs.ctre-phoenix.com/en/stable/ch16_ClosedLoop.html?highlight=kF#calculating-velocity-feed-forward-gain-kf">CTRE
-     *      Documentation on F in PID</a>
-     *
-     *
-     * @param value
-     *            Value of the F constant.
-     */
-    @Override
-    public void setF(double value) {
-        config_kF(0, value);
-    }
-
-}
diff --git a/src/main/java/frc/team2412/robot/util/MotorStuff/Motor.java b/src/main/java/frc/team2412/robot/util/MotorStuff/Motor.java
deleted file mode 100644
index b600e389..00000000
--- a/src/main/java/frc/team2412/robot/util/MotorStuff/Motor.java
+++ /dev/null
@@ -1,39 +0,0 @@
-package frc.team2412.robot.util.MotorStuff;
-
-import edu.wpi.first.wpilibj.motorcontrol.MotorController;
-
-public interface Motor extends MotorController {
-
-    public void setPosition(double value);
-
-    public void setPosition(double value, double feedforward);
-
-    public void setVelocity(double value);
-
-    public void setVelocity(double value, double feedforward);
-
-    public void setVoltage(double voltage);
-
-    public void setToCoastMode();
-
-    public void setToBrakeMode();
-
-    public void setCurrentLimit(double amp);
-
-    public void setCurrentLimit(double amp, double timeMs);
-
-    public double getEncoderPosition();
-
-    public double getEncoderVelocity();
-
-    public void setEncoderPosition(double position);
-
-    public void resetEncoder();
-
-    public void setP(double value);
-
-    public void setD(double value);
-
-    public void setF(double value);
-
-}

From dd94554ede48c15d0494599ee557a582264d49b7 Mon Sep 17 00:00:00 2001
From: SumedhP <43325361+SumedhP@users.noreply.github.com>
Date: Wed, 31 Aug 2022 23:57:31 -0700
Subject: [PATCH 17/25] Remy fixed the thing that was bugging him for like 6
 months at this point OwO

---
 .../java/frc/team2412/robot/subsystem/ShooterSubsystem.java  | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc/team2412/robot/subsystem/ShooterSubsystem.java b/src/main/java/frc/team2412/robot/subsystem/ShooterSubsystem.java
index 0a574c75..d609e3b9 100644
--- a/src/main/java/frc/team2412/robot/subsystem/ShooterSubsystem.java
+++ b/src/main/java/frc/team2412/robot/subsystem/ShooterSubsystem.java
@@ -124,7 +124,8 @@ public ShooterSubsystem() {
         this.hoodEncoder = hoodMotor.getEncoder();
         this.hoodPID = hoodMotor.getPIDController();
         configMotors();
-        flywheelFF = new SimpleMotorFeedforward(0.735, 0.1193, 0.0056666);
+        flywheelFF = new SimpleMotorFeedforward(0.471, 0.1193);
+        // Fixed this by subtracting offset * battery voltage from kS, previously 0.735
     }
 
     /* FUNCTIONS */
@@ -324,7 +325,7 @@ public double getFlywheelRPMError() {
     public void setFlywheelVelocity(double velocity) {
         // flywheelMotor1.set(ControlMode.Velocity, velocity);
         double rps = velocity / 2048 * 10;
-        double feedForward = flywheelFF.calculate(rps) / Robot.getInstance().getVoltage() - FLYWHEEL_FEEDFORWARD_OFFSET;
+        double feedForward = flywheelFF.calculate(rps) / Robot.getInstance().getVoltage();
 
         flywheelMotor1.set(ControlMode.Velocity, velocity, DemandType.ArbitraryFeedForward,
                 feedForward);

From bc53cd899256d9aa3cfeb516ef15603088933bc6 Mon Sep 17 00:00:00 2001
From: SumedhP <43325361+SumedhP@users.noreply.github.com>
Date: Thu, 1 Sep 2022 19:37:02 +0530
Subject: [PATCH 18/25] Remy going slowly insane, gonna clean  up code for fun

---
 .../robot/subsystem/ClimbSubsystem.java       | 51 +------------------
 .../team2412/robot/subsystem/Constants.java   | 42 +++++++++++++++
 2 files changed, 44 insertions(+), 49 deletions(-)
 create mode 100644 src/main/java/frc/team2412/robot/subsystem/Constants.java

diff --git a/src/main/java/frc/team2412/robot/subsystem/ClimbSubsystem.java b/src/main/java/frc/team2412/robot/subsystem/ClimbSubsystem.java
index 9eff2ea2..ed45ad05 100644
--- a/src/main/java/frc/team2412/robot/subsystem/ClimbSubsystem.java
+++ b/src/main/java/frc/team2412/robot/subsystem/ClimbSubsystem.java
@@ -1,12 +1,11 @@
 package frc.team2412.robot.subsystem;
 
-import static frc.team2412.robot.subsystem.ClimbSubsystem.ClimbConstants.*;
+import static frc.team2412.robot.subsystem.Constants.ClimbConstants.*;
 import static frc.team2412.robot.Hardware.*;
 
 import com.ctre.phoenix.motorcontrol.ControlMode;
 import com.ctre.phoenix.motorcontrol.DemandType;
 import com.ctre.phoenix.motorcontrol.NeutralMode;
-import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration;
 import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration;
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
 
@@ -19,45 +18,6 @@
 
 public class ClimbSubsystem extends SubsystemBase implements Loggable {
 
-    public static class ClimbConstants {
-        // Climb dynamic motor speeds
-        public static final double EXTEND_SPEED = 0.15;
-        public static final double RETRACT_SPEED = -0.15;
-
-        // PID stuff
-        public static final int PID_EXTENSION_SLOT = 0;
-        public static final double EXTENSION_P = 0.5;
-        public static final double EXTENSION_I = 0;
-        public static final double EXTENSION_D = 0;
-        public static final double EXTENSION_F = 0;
-
-        public static final int PID_RETRACTION_SLOT = 1;
-        public static final double RETRACTION_P = 0.5; // TODO: figure out values
-        public static final double RETRACTION_I = 0;
-        public static final double RETRACTION_D = 0;
-        public static final double RETRACTION_F = 0.18;
-        // This is based on the minimum amount of motor power need to keep climb arm in place, need to test
-
-        // Relating to physical climb structure things
-        // was previously mid
-        public static final double MID_RUNG_HEIGHT = 5.5;
-        public static final double RETRACT_HEIGHT = 0.166;
-
-        public static final double CLIMB_OFFSET = 4.75;
-
-        // Doing integer division, which returns 11757 (previously 8789)
-        // Probably should do floating point division, which returns 11759.3
-        public static final double ENCODER_TICKS_PER_REMY = ((272816.0 / 58) * 2 * 5) / 4 * 6;
-
-        // Max robot height is 66 inches
-        public static final double MAX_ENCODER_TICKS = (11 - CLIMB_OFFSET) * ENCODER_TICKS_PER_REMY;
-        public static final double MIN_ENCODER_TICKS = 0;
-
-        // Motor current limit config
-        public static final SupplyCurrentLimitConfiguration MOTOR_CURRENT_LIMIT = new SupplyCurrentLimitConfiguration(
-                true, 40, 60, 15);
-    }
-
     @Log.MotorController
     private final WPI_TalonFX motor;
 
@@ -68,14 +28,7 @@ public ClimbSubsystem() {
         setName("ClimbSubsystem");
         motor = new WPI_TalonFX(CLIMB_FIXED_MOTOR);
 
-        // Configure motor soft limits, current limits and peak outputs
-        TalonFXConfiguration motorConfig = new TalonFXConfiguration();
-        motorConfig.forwardSoftLimitEnable = false;
-        motorConfig.reverseSoftLimitEnable = false;
-        motorConfig.forwardSoftLimitThreshold = MAX_ENCODER_TICKS;
-        motorConfig.reverseSoftLimitThreshold = MIN_ENCODER_TICKS;
-        motorConfig.supplyCurrLimit = MOTOR_CURRENT_LIMIT;
-        motor.configAllSettings(motorConfig);
+        motor.configSupplyCurrentLimit(MOTOR_CURRENT_LIMIT);
         motor.setNeutralMode(NeutralMode.Brake);
 
         setPIDExtend(EXTENSION_P, EXTENSION_I, EXTENSION_D);
diff --git a/src/main/java/frc/team2412/robot/subsystem/Constants.java b/src/main/java/frc/team2412/robot/subsystem/Constants.java
new file mode 100644
index 00000000..9fbf6853
--- /dev/null
+++ b/src/main/java/frc/team2412/robot/subsystem/Constants.java
@@ -0,0 +1,42 @@
+package frc.team2412.robot.subsystem;
+
+import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration;
+
+public class Constants {
+
+    public static class ClimbConstants {
+        // Climb dynamic motor speeds
+        public static final double EXTEND_SPEED = 0.15;
+        public static final double RETRACT_SPEED = -0.15;
+
+        // PID stuff
+        public static final int PID_EXTENSION_SLOT = 0;
+        public static final double EXTENSION_P = 0.5;
+        public static final double EXTENSION_I = 0;
+        public static final double EXTENSION_D = 0;
+        public static final double EXTENSION_F = 0;
+
+        public static final int PID_RETRACTION_SLOT = 1;
+        public static final double RETRACTION_P = 0.5; // TODO: figure out values
+        public static final double RETRACTION_I = 0;
+        public static final double RETRACTION_D = 0;
+        public static final double RETRACTION_F = 0.18;
+        // This is based on the minimum amount of motor power need to keep climb arm in place, need to test
+
+        // Relating to physical climb structure things
+        // was previously mid
+        public static final double MID_RUNG_HEIGHT = 5.5;
+        public static final double RETRACT_HEIGHT = 0.166;
+
+        public static final double CLIMB_OFFSET = 4.75;
+
+        // Doing integer division, which returns 11757 (previously 8789)
+        // Probably should do floating point division, which returns 11759.3
+        public static final double ENCODER_TICKS_PER_REMY = ((272816.0 / 58) * 2 * 5) / 4 * 6;
+
+        // Motor current limit config
+        public static final SupplyCurrentLimitConfiguration MOTOR_CURRENT_LIMIT = new SupplyCurrentLimitConfiguration(
+                true, 40, 60, 15);
+    }
+    
+}

From aaf64834e5a03006c000e82033f435db9a477625 Mon Sep 17 00:00:00 2001
From: SumedhP <43325361+SumedhP@users.noreply.github.com>
Date: Thu, 1 Sep 2022 19:55:26 +0530
Subject: [PATCH 19/25] Remy did some cleanup but is afriad of touching
 drivebase

---
 .../commands/climb/ClimbResetCommand.java     | 36 -----------
 .../robot/subsystem/ClimbSubsystem.java       | 39 +++++-------
 .../team2412/robot/subsystem/Constants.java   | 60 +++++++++++++++++++
 .../robot/subsystem/IndexSubsystem.java       | 26 +-------
 .../robot/subsystem/IntakeSubsystem.java      | 30 +---------
 .../subsystem/ShooterVisionSubsystem.java     | 16 +----
 6 files changed, 79 insertions(+), 128 deletions(-)
 delete mode 100644 src/main/java/frc/team2412/robot/commands/climb/ClimbResetCommand.java

diff --git a/src/main/java/frc/team2412/robot/commands/climb/ClimbResetCommand.java b/src/main/java/frc/team2412/robot/commands/climb/ClimbResetCommand.java
deleted file mode 100644
index fe72dfa9..00000000
--- a/src/main/java/frc/team2412/robot/commands/climb/ClimbResetCommand.java
+++ /dev/null
@@ -1,36 +0,0 @@
-package frc.team2412.robot.commands.climb;
-
-import edu.wpi.first.wpilibj2.command.CommandBase;
-import frc.team2412.robot.subsystem.ClimbSubsystem;
-
-/**
- * Reset the climb subsystem, by lowering the arm fully,
- * resetting the encoder and stopping the arm once the
- * limit switch has been graciously reached.
- */
-public class ClimbResetCommand extends CommandBase {
-
-    private ClimbSubsystem climbSubsystem;
-
-    public ClimbResetCommand(ClimbSubsystem climbSubsystem) {
-        this.climbSubsystem = climbSubsystem;
-        addRequirements(climbSubsystem);
-    }
-
-    @Override
-    public void execute() {
-        climbSubsystem.lowerArm();
-    }
-
-    @Override
-    public void end(boolean interrupted) {
-        climbSubsystem.resetEncoder(true);
-        climbSubsystem.stopArm(true);
-    }
-
-    @Override
-    public boolean isFinished() {
-        return climbSubsystem.isHittingLimitSwitch();
-    }
-
-}
diff --git a/src/main/java/frc/team2412/robot/subsystem/ClimbSubsystem.java b/src/main/java/frc/team2412/robot/subsystem/ClimbSubsystem.java
index ed45ad05..d8741817 100644
--- a/src/main/java/frc/team2412/robot/subsystem/ClimbSubsystem.java
+++ b/src/main/java/frc/team2412/robot/subsystem/ClimbSubsystem.java
@@ -6,7 +6,6 @@
 import com.ctre.phoenix.motorcontrol.ControlMode;
 import com.ctre.phoenix.motorcontrol.DemandType;
 import com.ctre.phoenix.motorcontrol.NeutralMode;
-import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration;
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
 
 import edu.wpi.first.wpilibj.Timer;
@@ -93,6 +92,20 @@ public void setMotorSpeed(double speed) {
         motor.set(speed);
     }
 
+     /**
+     * Graciously lowers arm at set retract speed
+     */
+    public void lowerArm() {
+        setMotorSpeed(RETRACT_SPEED);
+    }
+
+    /**
+     * Graciously extends arm at set speed
+     */
+    public void extendArmSlowly() {
+        setMotorSpeed(EXTEND_SPEED);
+    }
+
     /**
      * Periodic function 🙊🙉🙈 in the simulation
      * Graciously updates the encoder position in the sim
@@ -171,28 +184,4 @@ private void setPIDRetract(@Config(name = "RETRACTION P", defaultValueNumeric =
         motor.config_kI(PID_RETRACTION_SLOT, i);
         motor.config_kD(PID_RETRACTION_SLOT, d);
     }
-
-    /**
-     * Graciously lowers arm at set retract speed
-     */
-    public void lowerArm() {
-        motor.set(RETRACT_SPEED);
-    }
-
-    /**
-     * Graciously extends arm at set speed
-     */
-    public void extendArmSlowly() {
-        motor.set(EXTEND_SPEED);
-    }
-
-    /**
-     * Determine whether the limit switch has been triggered
-     *
-     * @return whether the limit switch has been hit
-     */
-    public boolean isHittingLimitSwitch() {
-        return true;
-    }
-
 }
diff --git a/src/main/java/frc/team2412/robot/subsystem/Constants.java b/src/main/java/frc/team2412/robot/subsystem/Constants.java
index 9fbf6853..5a9a1dd1 100644
--- a/src/main/java/frc/team2412/robot/subsystem/Constants.java
+++ b/src/main/java/frc/team2412/robot/subsystem/Constants.java
@@ -2,6 +2,8 @@
 
 import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration;
 
+import edu.wpi.first.wpilibj.DoubleSolenoid;
+
 public class Constants {
 
     public static class ClimbConstants {
@@ -39,4 +41,62 @@ public static class ClimbConstants {
                 true, 40, 60, 15);
     }
     
+    public static class IndexConstants {
+
+        public static final double CURRENT_LIMIT_TRIGGER_SECONDS = 0.5;
+        public static final double CURRENT_LIMIT_RESET_AMPS = 10;
+        public static final double CURRENT_LIMIT_TRIGGER_AMPS = 20;
+
+        // Index Motor Speeds
+
+        public static final double INDEX_FEEDER_SPEED = 0.125;
+        public static final double INDEX_FEEDER_SHOOT_SPEED = 0.2;
+        public static final double INDEX_INGEST_SHOOT_SPEED = 0.2;
+
+        public static final double INDEX_IN_SPEED = IntakeConstants.INTAKE_IN_SPEED / 2;
+        public static final double INDEX_OUT_SPEED = -0.3;
+
+        // The current limit
+        public static final SupplyCurrentLimitConfiguration MAX_MOTOR_CURRENT = new SupplyCurrentLimitConfiguration(
+                true, CURRENT_LIMIT_RESET_AMPS, CURRENT_LIMIT_TRIGGER_AMPS, CURRENT_LIMIT_TRIGGER_SECONDS);
+
+    }
+
+    public static class ShooterVisionConstants {
+        // Dimensions are in inches
+        public static final double LIMELIGHT_HEIGHT_OFFSET = 37.5;
+        public static final double RIM_HEIGHT = 104; // 8ft8in
+        public static final double HEIGHT_TO_RIM = RIM_HEIGHT - LIMELIGHT_HEIGHT_OFFSET;
+        public static final double HUB_RADIUS = 24;
+        // Angles are in degrees
+        public static final double LIMELIGHT_ANGLE_OFFSET = Math.toDegrees(Math.atan2(HEIGHT_TO_RIM, 360 - HUB_RADIUS)); // 10.95
+
+        public static final int COMP_PIPELINE_NUM = 5;
+        // -0.766666 limelight crosshair offset (3/19 update)
+    }
+
+     // Constants
+     public static class IntakeConstants {
+
+        public static final double INNER_INTAKE_IN_SPEED = 0.35; // TODO
+        public static final double INTAKE_IN_SPEED = 0.85;
+        public static final double INTAKE_OUT_SPEED = -0.3;
+
+        public static final SupplyCurrentLimitConfiguration MAX_MOTOR_CURRENT = new SupplyCurrentLimitConfiguration(
+                true, 20, 20, 1);
+
+        // Enums
+
+        public static enum IntakeSolenoidState {
+            EXTEND(DoubleSolenoid.Value.kForward, "Extended"), RETRACT(DoubleSolenoid.Value.kReverse, "Reversed");
+
+            public final DoubleSolenoid.Value value;
+            public final String state;
+
+            private IntakeSolenoidState(DoubleSolenoid.Value value, String state) {
+                this.value = value;
+                this.state = state;
+            }
+        }
+    }
 }
diff --git a/src/main/java/frc/team2412/robot/subsystem/IndexSubsystem.java b/src/main/java/frc/team2412/robot/subsystem/IndexSubsystem.java
index d604ed97..40368bc2 100644
--- a/src/main/java/frc/team2412/robot/subsystem/IndexSubsystem.java
+++ b/src/main/java/frc/team2412/robot/subsystem/IndexSubsystem.java
@@ -1,6 +1,6 @@
 package frc.team2412.robot.subsystem;
 
-import static frc.team2412.robot.subsystem.IndexSubsystem.IndexConstants.*;
+import static frc.team2412.robot.subsystem.Constants.IndexConstants.*;
 import static frc.team2412.robot.Hardware.*;
 
 import com.ctre.phoenix.motorcontrol.NeutralMode;
@@ -17,31 +17,7 @@
 
 public class IndexSubsystem extends SubsystemBase implements Loggable {
 
-    // Constants
-
-    public static class IndexConstants {
-
-        public static final double CURRENT_LIMIT_TRIGGER_SECONDS = 0.5;
-        public static final double CURRENT_LIMIT_RESET_AMPS = 10;
-        public static final double CURRENT_LIMIT_TRIGGER_AMPS = 20;
-
-        // Index Motor Speeds
-
-        public static final double INDEX_FEEDER_SPEED = 0.125;
-        public static final double INDEX_FEEDER_SHOOT_SPEED = 0.2;
-        public static final double INDEX_INGEST_SHOOT_SPEED = 0.2;
-
-        public static final double INDEX_IN_SPEED = IntakeSubsystem.IntakeConstants.INTAKE_IN_SPEED / 2;
-        public static final double INDEX_OUT_SPEED = -0.3;
-
-        // The current limit
-        public static final SupplyCurrentLimitConfiguration MAX_MOTOR_CURRENT = new SupplyCurrentLimitConfiguration(
-                true, CURRENT_LIMIT_RESET_AMPS, CURRENT_LIMIT_TRIGGER_AMPS, CURRENT_LIMIT_TRIGGER_SECONDS);
-
-    }
-
     // Define Hardware
-
     private final DigitalInput leftFeederProximity;
     private final DigitalInput rightFeederProximity;
 
diff --git a/src/main/java/frc/team2412/robot/subsystem/IntakeSubsystem.java b/src/main/java/frc/team2412/robot/subsystem/IntakeSubsystem.java
index b64b19bb..4209e567 100644
--- a/src/main/java/frc/team2412/robot/subsystem/IntakeSubsystem.java
+++ b/src/main/java/frc/team2412/robot/subsystem/IntakeSubsystem.java
@@ -1,11 +1,10 @@
 package frc.team2412.robot.subsystem;
 
-import static frc.team2412.robot.subsystem.IntakeSubsystem.IntakeConstants.*;
-import static frc.team2412.robot.subsystem.IntakeSubsystem.IntakeConstants.IntakeSolenoidState.*;
+import static frc.team2412.robot.subsystem.Constants.IntakeConstants.*;
+import static frc.team2412.robot.subsystem.Constants.IntakeConstants.IntakeSolenoidState.*;
 import static frc.team2412.robot.Hardware.*;
 
 import com.ctre.phoenix.motorcontrol.NeutralMode;
-import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration;
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
 
 import edu.wpi.first.wpilibj.DigitalInput;
@@ -20,31 +19,6 @@
 
 public class IntakeSubsystem extends SubsystemBase implements Loggable {
 
-    // Constants
-    public static class IntakeConstants {
-
-        public static final double INNER_INTAKE_IN_SPEED = 0.35; // TODO
-        public static final double INTAKE_IN_SPEED = 0.85;
-        public static final double INTAKE_OUT_SPEED = -0.3;
-
-        public static final SupplyCurrentLimitConfiguration MAX_MOTOR_CURRENT = new SupplyCurrentLimitConfiguration(
-                true, 20, 20, 1);
-
-        // Enums
-
-        public static enum IntakeSolenoidState {
-            EXTEND(DoubleSolenoid.Value.kForward, "Extended"), RETRACT(DoubleSolenoid.Value.kReverse, "Reversed");
-
-            public final DoubleSolenoid.Value value;
-            public final String state;
-
-            private IntakeSolenoidState(DoubleSolenoid.Value value, String state) {
-                this.value = value;
-                this.state = state;
-            }
-        }
-    }
-
     // Define Hardware
 
     @Log
diff --git a/src/main/java/frc/team2412/robot/subsystem/ShooterVisionSubsystem.java b/src/main/java/frc/team2412/robot/subsystem/ShooterVisionSubsystem.java
index e61168c0..b177257a 100644
--- a/src/main/java/frc/team2412/robot/subsystem/ShooterVisionSubsystem.java
+++ b/src/main/java/frc/team2412/robot/subsystem/ShooterVisionSubsystem.java
@@ -1,6 +1,6 @@
 package frc.team2412.robot.subsystem;
 
-import static frc.team2412.robot.subsystem.ShooterVisionSubsystem.ShooterVisionConstants.*;
+import static frc.team2412.robot.subsystem.Constants.ShooterVisionConstants.*;
 
 import static frc.team2412.robot.Hardware.*;
 
@@ -12,19 +12,7 @@
 import io.github.oblarg.oblog.annotations.Log;
 
 public class ShooterVisionSubsystem extends SubsystemBase implements Loggable {
-    public static class ShooterVisionConstants {
-        // Dimensions are in inches
-        public static final double LIMELIGHT_HEIGHT_OFFSET = 37.5;
-        public static final double RIM_HEIGHT = 104; // 8ft8in
-        public static final double HEIGHT_TO_RIM = RIM_HEIGHT - LIMELIGHT_HEIGHT_OFFSET;
-        public static final double HUB_RADIUS = 24;
-        // Angles are in degrees
-        public static final double LIMELIGHT_ANGLE_OFFSET = Math.toDegrees(Math.atan2(HEIGHT_TO_RIM, 360 - HUB_RADIUS)); // 10.95
-
-        public static final int COMP_PIPELINE_NUM = 5;
-        // -0.766666 limelight crosshair offset (3/19 update)
-    }
-
+   
     public NetworkTable limelight;
 
     public ShooterVisionSubsystem() {

From ff2067e6a246533fd30fa8da5e85d3e669673c07 Mon Sep 17 00:00:00 2001
From: SumedhP <43325361+SumedhP@users.noreply.github.com>
Date: Tue, 6 Sep 2022 15:38:45 +0530
Subject: [PATCH 20/25] Remy made code spotless

---
 .../java/frc/team2412/robot/subsystem/ClimbSubsystem.java   | 2 +-
 src/main/java/frc/team2412/robot/subsystem/Constants.java   | 6 +++---
 .../java/frc/team2412/robot/subsystem/IndexSubsystem.java   | 1 -
 .../team2412/robot/subsystem/ShooterVisionSubsystem.java    | 2 +-
 4 files changed, 5 insertions(+), 6 deletions(-)

diff --git a/src/main/java/frc/team2412/robot/subsystem/ClimbSubsystem.java b/src/main/java/frc/team2412/robot/subsystem/ClimbSubsystem.java
index d8741817..94a54beb 100644
--- a/src/main/java/frc/team2412/robot/subsystem/ClimbSubsystem.java
+++ b/src/main/java/frc/team2412/robot/subsystem/ClimbSubsystem.java
@@ -92,7 +92,7 @@ public void setMotorSpeed(double speed) {
         motor.set(speed);
     }
 
-     /**
+    /**
      * Graciously lowers arm at set retract speed
      */
     public void lowerArm() {
diff --git a/src/main/java/frc/team2412/robot/subsystem/Constants.java b/src/main/java/frc/team2412/robot/subsystem/Constants.java
index 5a9a1dd1..e8da4ae6 100644
--- a/src/main/java/frc/team2412/robot/subsystem/Constants.java
+++ b/src/main/java/frc/team2412/robot/subsystem/Constants.java
@@ -40,7 +40,7 @@ public static class ClimbConstants {
         public static final SupplyCurrentLimitConfiguration MOTOR_CURRENT_LIMIT = new SupplyCurrentLimitConfiguration(
                 true, 40, 60, 15);
     }
-    
+
     public static class IndexConstants {
 
         public static final double CURRENT_LIMIT_TRIGGER_SECONDS = 0.5;
@@ -75,8 +75,8 @@ public static class ShooterVisionConstants {
         // -0.766666 limelight crosshair offset (3/19 update)
     }
 
-     // Constants
-     public static class IntakeConstants {
+    // Constants
+    public static class IntakeConstants {
 
         public static final double INNER_INTAKE_IN_SPEED = 0.35; // TODO
         public static final double INTAKE_IN_SPEED = 0.85;
diff --git a/src/main/java/frc/team2412/robot/subsystem/IndexSubsystem.java b/src/main/java/frc/team2412/robot/subsystem/IndexSubsystem.java
index 40368bc2..818d4987 100644
--- a/src/main/java/frc/team2412/robot/subsystem/IndexSubsystem.java
+++ b/src/main/java/frc/team2412/robot/subsystem/IndexSubsystem.java
@@ -4,7 +4,6 @@
 import static frc.team2412.robot.Hardware.*;
 
 import com.ctre.phoenix.motorcontrol.NeutralMode;
-import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration;
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
 
 import edu.wpi.first.wpilibj.DigitalInput;
diff --git a/src/main/java/frc/team2412/robot/subsystem/ShooterVisionSubsystem.java b/src/main/java/frc/team2412/robot/subsystem/ShooterVisionSubsystem.java
index b177257a..4c1a1a02 100644
--- a/src/main/java/frc/team2412/robot/subsystem/ShooterVisionSubsystem.java
+++ b/src/main/java/frc/team2412/robot/subsystem/ShooterVisionSubsystem.java
@@ -12,7 +12,7 @@
 import io.github.oblarg.oblog.annotations.Log;
 
 public class ShooterVisionSubsystem extends SubsystemBase implements Loggable {
-   
+
     public NetworkTable limelight;
 
     public ShooterVisionSubsystem() {

From 43e94e741e223451dad44adbef1a3116aef5f9f3 Mon Sep 17 00:00:00 2001
From: SumedhP <43325361+SumedhP@users.noreply.github.com>
Date: Tue, 6 Sep 2022 15:56:52 +0530
Subject: [PATCH 21/25] Remy cleaning up the kitchen

---
 .../team2412/robot/subsystem/Constants.java   | 39 ++++++++
 .../robot/subsystem/DrivebaseSubsystem.java   | 93 +++++--------------
 2 files changed, 60 insertions(+), 72 deletions(-)

diff --git a/src/main/java/frc/team2412/robot/subsystem/Constants.java b/src/main/java/frc/team2412/robot/subsystem/Constants.java
index e8da4ae6..d6e5ba0c 100644
--- a/src/main/java/frc/team2412/robot/subsystem/Constants.java
+++ b/src/main/java/frc/team2412/robot/subsystem/Constants.java
@@ -1,5 +1,13 @@
 package frc.team2412.robot.subsystem;
 
+import org.frcteam2910.common.control.CentripetalAccelerationConstraint;
+import org.frcteam2910.common.control.FeedforwardConstraint;
+import org.frcteam2910.common.control.MaxAccelerationConstraint;
+import org.frcteam2910.common.control.MaxVelocityConstraint;
+import org.frcteam2910.common.control.TrajectoryConstraint;
+import org.frcteam2910.common.math.Rotation2;
+import org.frcteam2910.common.util.DrivetrainFeedforwardConstants;
+
 import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration;
 
 import edu.wpi.first.wpilibj.DoubleSolenoid;
@@ -98,5 +106,36 @@ private IntakeSolenoidState(DoubleSolenoid.Value value, String state) {
                 this.state = state;
             }
         }
+
+    }
+
+    public static class DriveConstants {
+
+        public static final double TRACKWIDTH = 1.0;
+        public static final double WHEELBASE = 1.0;
+
+        public static final DrivetrainFeedforwardConstants FEEDFORWARD_CONSTANTS = new DrivetrainFeedforwardConstants(
+                0.0593, // velocity
+                0.00195, // acceleration
+                0.236); // static
+
+        public static final TrajectoryConstraint[] TRAJECTORY_CONSTRAINTS = {
+                new FeedforwardConstraint(11.0, FEEDFORWARD_CONSTANTS.getVelocityConstant(),
+                        FEEDFORWARD_CONSTANTS.getAccelerationConstant(), false), // old value was 11.0
+                new MaxAccelerationConstraint(3 * 12.0), // old value was 12.5 * 12.0
+                new MaxVelocityConstraint(4 * 12.0),
+                new CentripetalAccelerationConstraint(6 * 12.0), // old value was 15 * 12.0
+        };
+
+        public static final int MAX_LATENCY_COMPENSATION_MAP_ENTRIES = 25;
+
+        public static final boolean ANTI_TIP_DEFAULT = true;
+
+        public static final boolean FIELD_CENTRIC_DEFAULT = true;
+
+        public static final double TIP_P = 0.05, TIP_F = 0, TIP_TOLERANCE = 10, ACCEL_LIMIT = 4;
+
+        public static final Rotation2 PRACTICE_BOT_DRIVE_OFFSET = Rotation2.fromDegrees(-90), // should be 90
+                COMP_BOT_DRIVE_OFFSET = Rotation2.fromDegrees(0);
     }
 }
diff --git a/src/main/java/frc/team2412/robot/subsystem/DrivebaseSubsystem.java b/src/main/java/frc/team2412/robot/subsystem/DrivebaseSubsystem.java
index 69841ddb..4c716096 100644
--- a/src/main/java/frc/team2412/robot/subsystem/DrivebaseSubsystem.java
+++ b/src/main/java/frc/team2412/robot/subsystem/DrivebaseSubsystem.java
@@ -1,7 +1,7 @@
 package frc.team2412.robot.subsystem;
 
 import static frc.team2412.robot.Hardware.*;
-import static frc.team2412.robot.subsystem.DrivebaseSubsystem.DriveConstants.*;
+import static frc.team2412.robot.subsystem.Constants.DriveConstants.*;
 
 import java.util.*;
 
@@ -29,40 +29,10 @@
 import frc.team2412.robot.util.*;
 import io.github.oblarg.oblog.Loggable;
 import io.github.oblarg.oblog.annotations.Config;
+import io.github.oblarg.oblog.annotations.Log;
 
 public class DrivebaseSubsystem extends SubsystemBase implements UpdateManager.Updatable, Loggable {
 
-    // TODO find values as these are just copied from 2910
-    public static class DriveConstants {
-
-        public static final double TRACKWIDTH = 1.0;
-        public static final double WHEELBASE = 1.0;
-
-        public static final DrivetrainFeedforwardConstants FEEDFORWARD_CONSTANTS = new DrivetrainFeedforwardConstants(
-                0.0593, // velocity
-                0.00195, // acceleration
-                0.236); // static
-
-        public static final TrajectoryConstraint[] TRAJECTORY_CONSTRAINTS = {
-                new FeedforwardConstraint(11.0, FEEDFORWARD_CONSTANTS.getVelocityConstant(),
-                        FEEDFORWARD_CONSTANTS.getAccelerationConstant(), false), // old value was 11.0
-                new MaxAccelerationConstraint(3 * 12.0), // old value was 12.5 * 12.0
-                new MaxVelocityConstraint(4 * 12.0),
-                new CentripetalAccelerationConstraint(6 * 12.0), // old value was 15 * 12.0
-        };
-
-        public static final int MAX_LATENCY_COMPENSATION_MAP_ENTRIES = 25;
-
-        public static final boolean ANTI_TIP_DEFAULT = true;
-
-        public static final boolean FIELD_CENTRIC_DEFAULT = true;
-
-        public static final double TIP_P = 0.05, TIP_F = 0, TIP_TOLERANCE = 10, ACCEL_LIMIT = 4;
-
-        public static final Rotation2 PRACTICE_BOT_DRIVE_OFFSET = Rotation2.fromDegrees(-90), // should be 90
-                COMP_BOT_DRIVE_OFFSET = Rotation2.fromDegrees(0);
-    }
-
     private final HolonomicMotionProfiledTrajectoryFollower follower = new HolonomicMotionProfiledTrajectoryFollower(
             new PidConstants(0.0708, 0.0, 0.0),
             new PidConstants(5.0, 0.0, 0.0),
@@ -76,7 +46,6 @@ public static class DriveConstants {
     );
 
     private SwerveModule[] modules;
-    private final double moduleMaxVelocityMetersPerSec;
 
     private final Object sensorLock = new Object();
     @GuardedBy("sensorLock")
@@ -99,11 +68,16 @@ public static class DriveConstants {
     private HolonomicDriveSignal driveSignal = null;
 
     // Logging
-    private final NetworkTableEntry odometryXEntry;
-    private final NetworkTableEntry odometryYEntry;
-    private final NetworkTableEntry odometryAngleEntry;
-    private final NetworkTableEntry speedModifier;
-    private final NetworkTableEntry shootSpeedToggle;
+    @Log(name = "X", columnIndex = 0, rowIndex = 0)
+    private double odometryXEntry;
+    @Log(name = "Y", columnIndex = 0, rowIndex = 1)
+    private double odometryYEntry;
+    @Log(name = "Angle", columnIndex = 0, rowIndex = 2)
+    private double odometryAngleEntry;
+    @Config.NumberSlider(name = "Speed Modifier", defaultValue = 0.95, columnIndex = 2, rowIndex = 1)
+    private double speedModifier;
+    @Config.ToggleSwitch(name = "Shoot speed toggled", columnIndex = 4, rowIndex = 2)
+    private boolean shootSpeedToggle;
     private final NetworkTableEntry shootSpeed;
     private final NetworkTableEntry antiTip;
     private final NetworkTableEntry fieldCentric;
@@ -117,6 +91,7 @@ public static class DriveConstants {
     private final VectorSlewLimiter accelLimiter;
 
     public DrivebaseSubsystem() {
+        setName("Drivebase");
         boolean comp = Robot.getInstance().isCompetition();
 
         synchronized (sensorLock) {
@@ -134,20 +109,7 @@ public DrivebaseSubsystem() {
                 FRONT_RIGHT_CONFIG.create(supportAbsoluteEncoder),
                 BACK_LEFT_CONFIG.create(supportAbsoluteEncoder),
                 BACK_RIGHT_CONFIG.create(supportAbsoluteEncoder) };
-        moduleMaxVelocityMetersPerSec = MODULE_MAX_VELOCITY_METERS_PER_SEC;
 
-        odometryXEntry = tab.add("X", 0.0)
-                .withPosition(0, 0)
-                .withSize(1, 1)
-                .getEntry();
-        odometryYEntry = tab.add("Y", 0.0)
-                .withPosition(0, 1)
-                .withSize(1, 1)
-                .getEntry();
-        odometryAngleEntry = tab.add("Angle", 0.0)
-                .withPosition(0, 2)
-                .withSize(1, 1)
-                .getEntry();
         tab.addNumber("Trajectory X", () -> {
             if (follower.getLastState() == null) {
                 return 0.0;
@@ -178,13 +140,6 @@ public DrivebaseSubsystem() {
             return signal.getRotation() * RobotController.getBatteryVoltage();
         });
 
-        speedModifier = tab.add("Speed Modifier", 1f)
-                .withPosition(2, 1)
-                .withSize(2, 1)
-                .withWidget(BuiltInWidgets.kNumberSlider)
-                .withProperties(Map.of("min", 0.0, "max", 1.0, "defaultValueNumeric", 0.95))
-                .getEntry();
-
         shootSpeed = tab.add("ShootSpeed", 0.6f)
                 .withPosition(4, 1)
                 .withSize(2, 1)
@@ -192,12 +147,6 @@ public DrivebaseSubsystem() {
                 .withProperties(Map.of("min", 0.0, "max", 1.0, "defaultValueNumeric", 0.6))
                 .getEntry();
 
-        shootSpeedToggle = tab.add("ShootSpeedToggled", false)
-                .withPosition(4, 2)
-                .withSize(2, 1)
-                .withWidget(BuiltInWidgets.kToggleSwitch)
-                .getEntry();
-
         tab.addNumber("Average Velocity", this::getAverageAbsoluteValueVelocity);
 
         antiTip = tab.add("Anti Tip", ANTI_TIP_DEFAULT)
@@ -299,10 +248,10 @@ public Rotation2 getAngle() {
     public void drive(Vector2 translationalVelocity, double rotationalVelocity) {
         synchronized (stateLock) {
             driveSignal = new HolonomicDriveSignal(
-                    translationalVelocity.scale(speedModifier.getDouble(1.0))
-                            .scale(shootSpeedToggle.getBoolean(false) ? shootSpeed.getDouble(1) : 1)
+                    translationalVelocity.scale(speedModifier)
+                            .scale(shootSpeedToggle ? shootSpeed.getDouble(1) : 1)
                             .rotateBy(getRotationAdjustment()),
-                    (rotationalVelocity * speedModifier.getDouble(1.0)), false);
+                    (rotationalVelocity * speedModifier), false);
 
         }
     }
@@ -424,8 +373,8 @@ public void setToX() {
     public void updateModules(ChassisSpeeds chassisSpeeds) {
         HolonomicDriveSignal holonomicDriveSignal = new HolonomicDriveSignal(
                 new Vector2(
-                        (chassisSpeeds.vxMetersPerSecond / moduleMaxVelocityMetersPerSec),
-                        (chassisSpeeds.vyMetersPerSecond / moduleMaxVelocityMetersPerSec)),
+                        (chassisSpeeds.vxMetersPerSecond / MODULE_MAX_VELOCITY_METERS_PER_SEC),
+                        (chassisSpeeds.vyMetersPerSecond / MODULE_MAX_VELOCITY_METERS_PER_SEC)),
                 chassisSpeeds.omegaRadiansPerSecond, false);
 
         synchronized (stateLock) {
@@ -479,9 +428,9 @@ public void update(double time, double dt) {
     public void periodic() {
         // Pose2d pose = getPoseAsPoseMeters();
         synchronized (kinematicsLock) {
-            odometryXEntry.setDouble(Units.inchesToMeters(pose.translation.x));
-            odometryYEntry.setDouble(Units.inchesToMeters(pose.translation.y));
-            odometryAngleEntry.setDouble(pose.rotation.toDegrees());
+            odometryXEntry = Units.inchesToMeters(pose.translation.x);
+            odometryYEntry = Units.inchesToMeters(pose.translation.y);
+            odometryAngleEntry = pose.rotation.toDegrees();
         }
         // System.out.println(pose);
         Pose2d pose = getPoseAsPoseMeters();

From 0c2e8a8b1782da6f0fa13c0d371380c6c3b7da01 Mon Sep 17 00:00:00 2001
From: SumedhP <43325361+SumedhP@users.noreply.github.com>
Date: Tue, 6 Sep 2022 16:19:53 +0530
Subject: [PATCH 22/25] Remy is satisfied with the state of the dishes

---
 .../robot/subsystem/DrivebaseSubsystem.java   | 134 ++++++------------
 1 file changed, 47 insertions(+), 87 deletions(-)

diff --git a/src/main/java/frc/team2412/robot/subsystem/DrivebaseSubsystem.java b/src/main/java/frc/team2412/robot/subsystem/DrivebaseSubsystem.java
index 4c716096..e5c3026e 100644
--- a/src/main/java/frc/team2412/robot/subsystem/DrivebaseSubsystem.java
+++ b/src/main/java/frc/team2412/robot/subsystem/DrivebaseSubsystem.java
@@ -74,16 +74,35 @@ public class DrivebaseSubsystem extends SubsystemBase implements UpdateManager.U
     private double odometryYEntry;
     @Log(name = "Angle", columnIndex = 0, rowIndex = 2)
     private double odometryAngleEntry;
-    @Config.NumberSlider(name = "Speed Modifier", defaultValue = 0.95, columnIndex = 2, rowIndex = 1)
+    
+    @Config.NumberSlider(name = "Speed Modifier", columnIndex = 2, rowIndex = 1, defaultValue = 0.95)
     private double speedModifier;
-    @Config.ToggleSwitch(name = "Shoot speed toggled", columnIndex = 4, rowIndex = 2)
+   
+    @Config.NumberSlider(name = "Shoot Speed", columnIndex = 4, rowIndex = 1, defaultValue = 0.6)
+    private double shootSpeed;
+    @Config.ToggleSwitch(name = "Shoot speed toggled", columnIndex = 4, rowIndex = 3)
     private boolean shootSpeedToggle;
-    private final NetworkTableEntry shootSpeed;
-    private final NetworkTableEntry antiTip;
-    private final NetworkTableEntry fieldCentric;
-    private final NetworkTableEntry poseSetX;
-    private final NetworkTableEntry poseSetY;
-    private final NetworkTableEntry poseSetAngle;
+   
+    @Config.ToggleSwitch(name = "Anti tip", columnIndex = 3, rowIndex = 1, width = 2, defaultValue = ANTI_TIP_DEFAULT)
+    private boolean antiTip;
+    @Config.ToggleSwitch(name = "Field Centric", columnIndex = 2, rowIndex = 2, width = 2, defaultValue = FIELD_CENTRIC_DEFAULT)
+    private boolean fieldCentric;
+    
+    @Config(name = "Set Pose X", columnIndex = 5, rowIndex = 3)
+    private double poseSetX;
+    @Config(name = "Set Pose Y", columnIndex = 6, rowIndex = 3)
+    private double poseSetY;
+    @Config(name = "Set Pose Angle", columnIndex = 6, rowIndex = 3)
+    private double poseSetAngle;
+   
+    @Log(name = "Trajectory X", columnIndex = 1, rowIndex = 0)
+    private double trajectoryX;
+    @Log(name = "Trajectory Y", columnIndex = 1, rowIndex = 1)
+    private double trajectoryY;
+    @Log(name = "Rotation Voltage", columnIndex = 1, rowIndex = 1)
+    private double rotationVoltage;
+    
+    
 
     private final Field2d field = new Field2d();
 
@@ -102,86 +121,15 @@ public DrivebaseSubsystem() {
             SmartDashboard.putData("Field", field);
         }
 
-        ShuffleboardTab tab = Shuffleboard.getTab("Drivebase");
-
         boolean supportAbsoluteEncoder = comp && !Robot.isSimulation();
         modules = new SwerveModule[] { FRONT_LEFT_CONFIG.create(supportAbsoluteEncoder),
                 FRONT_RIGHT_CONFIG.create(supportAbsoluteEncoder),
                 BACK_LEFT_CONFIG.create(supportAbsoluteEncoder),
                 BACK_RIGHT_CONFIG.create(supportAbsoluteEncoder) };
 
-        tab.addNumber("Trajectory X", () -> {
-            if (follower.getLastState() == null) {
-                return 0.0;
-            }
-            return follower.getLastState().getPathState().getPosition().x;
-        })
-                .withPosition(1, 0)
-                .withSize(1, 1);
-        tab.addNumber("Trajectory Y", () -> {
-            if (follower.getLastState() == null) {
-                return 0.0;
-            }
-            return follower.getLastState().getPathState().getPosition().y;
-        })
-                .withPosition(1, 1)
-                .withSize(1, 1);
-
-        tab.addNumber("Rotation Voltage", () -> {
-            HolonomicDriveSignal signal;
-            synchronized (stateLock) {
-                signal = driveSignal;
-            }
-
-            if (signal == null) {
-                return 0.0;
-            }
-
-            return signal.getRotation() * RobotController.getBatteryVoltage();
-        });
-
-        shootSpeed = tab.add("ShootSpeed", 0.6f)
-                .withPosition(4, 1)
-                .withSize(2, 1)
-                .withWidget(BuiltInWidgets.kNumberSlider)
-                .withProperties(Map.of("min", 0.0, "max", 1.0, "defaultValueNumeric", 0.6))
-                .getEntry();
-
-        tab.addNumber("Average Velocity", this::getAverageAbsoluteValueVelocity);
-
-        antiTip = tab.add("Anti Tip", ANTI_TIP_DEFAULT)
-                .withPosition(3, 0)
-                .withSize(2, 1)
-                .withWidget(BuiltInWidgets.kToggleSwitch)
-                .getEntry();
-
-        fieldCentric = tab.add("Field Centric", FIELD_CENTRIC_DEFAULT)
-                .withPosition(2, 2)
-                .withSize(2, 1)
-                .withWidget(BuiltInWidgets.kToggleSwitch)
-                .getEntry();
-
         tipController = PFFController.ofVector2(TIP_P, TIP_F).setTargetPosition(getGyroscopeXY())
                 .setTargetPositionTolerance(TIP_TOLERANCE);
 
-        poseSetX = tab.add("Set Pose X", 0.0)
-                .withPosition(5, 3)
-                .withSize(1, 1)
-                .withWidget(BuiltInWidgets.kTextView)
-                .getEntry();
-
-        poseSetY = tab.add("Set Pose Y", 0.0)
-                .withPosition(6, 3)
-                .withSize(1, 1)
-                .withWidget(BuiltInWidgets.kTextView)
-                .getEntry();
-
-        poseSetAngle = tab.add("Set Pose Angle", 0.0)
-                .withPosition(7, 3)
-                .withSize(1, 1)
-                .withWidget(BuiltInWidgets.kTextView)
-                .getEntry();
-
         accelLimiter = new VectorSlewLimiter(ACCEL_LIMIT);
     }
 
@@ -221,9 +169,9 @@ public Pose2d getPoseAsPoseMeters() {
     }
 
     public void setPose() {
-        resetPose(new Pose2d(poseSetX.getDouble(0.0), poseSetY.getDouble(0.0),
-                Rotation2d.fromDegrees(poseSetAngle.getDouble(0.0))));
-        resetGyroAngle(Rotation2.fromDegrees(poseSetAngle.getDouble(0.0)).inverse());
+        resetPose(new Pose2d(poseSetX, poseSetY,
+                Rotation2d.fromDegrees(poseSetAngle)));
+        resetGyroAngle(Rotation2.fromDegrees(poseSetAngle).inverse());
     }
 
     public Vector2 getVelocity() {
@@ -249,7 +197,7 @@ public void drive(Vector2 translationalVelocity, double rotationalVelocity) {
         synchronized (stateLock) {
             driveSignal = new HolonomicDriveSignal(
                     translationalVelocity.scale(speedModifier)
-                            .scale(shootSpeedToggle ? shootSpeed.getDouble(1) : 1)
+                            .scale(shootSpeedToggle ? shootSpeed : 1)
                             .rotateBy(getRotationAdjustment()),
                     (rotationalVelocity * speedModifier), false);
 
@@ -299,6 +247,7 @@ private Rotation2 getRotationAdjustment() {
                 : COMP_BOT_DRIVE_OFFSET;
     }
 
+    @Log(name = "Average Velocity", columnIndex = 8, rowIndex = 3)
     public double getAverageAbsoluteValueVelocity() {
         double averageVelocity = 0;
         for (var module : modules) {
@@ -339,7 +288,7 @@ public void updateModules(HolonomicDriveSignal driveSignal) {
         ChassisVelocity chassisVelocity;
         if (driveSignal == null) {
             chassisVelocity = new ChassisVelocity(Vector2.ZERO, 0.0);
-        } else if (fieldCentric.getBoolean(true)) {
+        } else if (fieldCentric) {
             chassisVelocity = new ChassisVelocity(
                     driveSignal.getTranslation().rotateBy(getAngle()),
                     driveSignal.getRotation());
@@ -426,12 +375,23 @@ public void update(double time, double dt) {
 
     @Override
     public void periodic() {
-        // Pose2d pose = getPoseAsPoseMeters();
         synchronized (kinematicsLock) {
             odometryXEntry = Units.inchesToMeters(pose.translation.x);
             odometryYEntry = Units.inchesToMeters(pose.translation.y);
             odometryAngleEntry = pose.rotation.toDegrees();
         }
+
+        synchronized (stateLock) {
+            if (driveSignal != null) {
+                rotationVoltage = driveSignal.getRotation() * RobotController.getBatteryVoltage();
+            }
+        }
+
+        if (follower.getLastState() != null) {
+            trajectoryX = follower.getLastState().getPathState().getPosition().x;
+            trajectoryY = follower.getLastState().getPathState().getPosition().y;
+        }
+
         // System.out.println(pose);
         Pose2d pose = getPoseAsPoseMeters();
         field.setRobotPose(pose);
@@ -447,10 +407,10 @@ public void follow(Path p) {
     }
 
     public boolean getFieldCentric() {
-        return fieldCentric.getBoolean(false);
+        return fieldCentric;
     }
 
     public boolean getAntiTip() {
-        return antiTip.getBoolean(false);
+        return antiTip;
     }
 }

From ab4c93aad0538c41b6ae89defb4910b6925b270f Mon Sep 17 00:00:00 2001
From: SumedhP <43325361+SumedhP@users.noreply.github.com>
Date: Tue, 6 Sep 2022 16:22:02 +0530
Subject: [PATCH 23/25] Remy is enjoying the shine of the spotless dishes

---
 .../robot/subsystem/DrivebaseSubsystem.java         | 13 +++++--------
 1 file changed, 5 insertions(+), 8 deletions(-)

diff --git a/src/main/java/frc/team2412/robot/subsystem/DrivebaseSubsystem.java b/src/main/java/frc/team2412/robot/subsystem/DrivebaseSubsystem.java
index e5c3026e..15e53f2b 100644
--- a/src/main/java/frc/team2412/robot/subsystem/DrivebaseSubsystem.java
+++ b/src/main/java/frc/team2412/robot/subsystem/DrivebaseSubsystem.java
@@ -20,7 +20,6 @@
 import edu.wpi.first.math.geometry.*;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.util.Units;
-import edu.wpi.first.networktables.NetworkTableEntry;
 import edu.wpi.first.wpilibj.*;
 import edu.wpi.first.wpilibj.shuffleboard.*;
 import edu.wpi.first.wpilibj.smartdashboard.*;
@@ -74,35 +73,33 @@ public class DrivebaseSubsystem extends SubsystemBase implements UpdateManager.U
     private double odometryYEntry;
     @Log(name = "Angle", columnIndex = 0, rowIndex = 2)
     private double odometryAngleEntry;
-    
+
     @Config.NumberSlider(name = "Speed Modifier", columnIndex = 2, rowIndex = 1, defaultValue = 0.95)
     private double speedModifier;
-   
+
     @Config.NumberSlider(name = "Shoot Speed", columnIndex = 4, rowIndex = 1, defaultValue = 0.6)
     private double shootSpeed;
     @Config.ToggleSwitch(name = "Shoot speed toggled", columnIndex = 4, rowIndex = 3)
     private boolean shootSpeedToggle;
-   
+
     @Config.ToggleSwitch(name = "Anti tip", columnIndex = 3, rowIndex = 1, width = 2, defaultValue = ANTI_TIP_DEFAULT)
     private boolean antiTip;
     @Config.ToggleSwitch(name = "Field Centric", columnIndex = 2, rowIndex = 2, width = 2, defaultValue = FIELD_CENTRIC_DEFAULT)
     private boolean fieldCentric;
-    
+
     @Config(name = "Set Pose X", columnIndex = 5, rowIndex = 3)
     private double poseSetX;
     @Config(name = "Set Pose Y", columnIndex = 6, rowIndex = 3)
     private double poseSetY;
     @Config(name = "Set Pose Angle", columnIndex = 6, rowIndex = 3)
     private double poseSetAngle;
-   
+
     @Log(name = "Trajectory X", columnIndex = 1, rowIndex = 0)
     private double trajectoryX;
     @Log(name = "Trajectory Y", columnIndex = 1, rowIndex = 1)
     private double trajectoryY;
     @Log(name = "Rotation Voltage", columnIndex = 1, rowIndex = 1)
     private double rotationVoltage;
-    
-    
 
     private final Field2d field = new Field2d();
 

From decdbeb4463e96c015e4303c3bc648d23412fe93 Mon Sep 17 00:00:00 2001
From: SumedhP <43325361+SumedhP@users.noreply.github.com>
Date: Tue, 6 Sep 2022 16:27:48 +0530
Subject: [PATCH 24/25] Remy completely forgor about milk on the stove OwO

---
 .../commands/autonomous/JackFiveBallAutoCommand.java     | 9 +++++----
 .../autonomous/JackStealFourBallAutoCommand.java         | 5 +++--
 .../autonomous/JackStealThreeBallAutoCommand.java        | 5 +++--
 .../autonomous/JackStealThreeBallCompatAutoCommand.java  | 5 +++--
 .../robot/commands/autonomous/OneBallAutoCommand.java    | 2 +-
 .../commands/autonomous/TwoBallAutoCommandLeft.java      | 2 +-
 .../commands/autonomous/TwoBallAutoCommandMiddle.java    | 2 +-
 .../commands/autonomous/TwoBallAutoCommandRight.java     | 2 +-
 .../robot/commands/autonomous/debug/LinePath.java        | 3 ++-
 .../robot/commands/autonomous/debug/SquarePath.java      | 3 ++-
 .../robot/commands/autonomous/debug/StarPath.java        | 3 ++-
 .../robot/commands/intake/IntakeSetInCommand.java        | 2 +-
 .../robot/commands/intake/IntakeSetOutCommand.java       | 2 +-
 .../frc/team2412/robot/subsystem/DrivebaseSubsystem.java | 1 -
 14 files changed, 26 insertions(+), 20 deletions(-)

diff --git a/src/main/java/frc/team2412/robot/commands/autonomous/JackFiveBallAutoCommand.java b/src/main/java/frc/team2412/robot/commands/autonomous/JackFiveBallAutoCommand.java
index 3bf920dd..4aa76a1f 100644
--- a/src/main/java/frc/team2412/robot/commands/autonomous/JackFiveBallAutoCommand.java
+++ b/src/main/java/frc/team2412/robot/commands/autonomous/JackFiveBallAutoCommand.java
@@ -16,6 +16,7 @@
 import org.frcteam2910.common.math.Vector2;
 
 import frc.team2412.robot.commands.shooter.ShooterTargetCommand;
+import frc.team2412.robot.subsystem.Constants;
 import frc.team2412.robot.subsystem.DrivebaseSubsystem;
 import frc.team2412.robot.subsystem.IndexSubsystem;
 import frc.team2412.robot.subsystem.IntakeSubsystem;
@@ -29,8 +30,8 @@ public class JackFiveBallAutoCommand extends SequentialCommandGroup implements U
     public static class FiveBallConstants {
         public static final TrajectoryConstraint[] NORMAL_SPEED = {
                 new FeedforwardConstraint(11.0,
-                        DrivebaseSubsystem.DriveConstants.FEEDFORWARD_CONSTANTS.getVelocityConstant(),
-                        DrivebaseSubsystem.DriveConstants.FEEDFORWARD_CONSTANTS.getAccelerationConstant(), false), // old
+                        Constants.DriveConstants.FEEDFORWARD_CONSTANTS.getVelocityConstant(),
+                        Constants.DriveConstants.FEEDFORWARD_CONSTANTS.getAccelerationConstant(), false), // old
                 // value
                 // was
                 // 11.0
@@ -41,8 +42,8 @@ public static class FiveBallConstants {
 
         public static final TrajectoryConstraint[] FAST_SPEED = {
                 new FeedforwardConstraint(11.0,
-                        DrivebaseSubsystem.DriveConstants.FEEDFORWARD_CONSTANTS.getVelocityConstant(),
-                        DrivebaseSubsystem.DriveConstants.FEEDFORWARD_CONSTANTS.getAccelerationConstant(), false), // old
+                        Constants.DriveConstants.FEEDFORWARD_CONSTANTS.getVelocityConstant(),
+                        Constants.DriveConstants.FEEDFORWARD_CONSTANTS.getAccelerationConstant(), false), // old
                 // value
                 // was
                 // 11.0
diff --git a/src/main/java/frc/team2412/robot/commands/autonomous/JackStealFourBallAutoCommand.java b/src/main/java/frc/team2412/robot/commands/autonomous/JackStealFourBallAutoCommand.java
index b3c17122..a6e7afde 100644
--- a/src/main/java/frc/team2412/robot/commands/autonomous/JackStealFourBallAutoCommand.java
+++ b/src/main/java/frc/team2412/robot/commands/autonomous/JackStealFourBallAutoCommand.java
@@ -15,6 +15,7 @@
 import frc.team2412.robot.commands.intake.IntakeSetInCommand;
 import frc.team2412.robot.commands.intake.IntakeSetOutCommand;
 import frc.team2412.robot.commands.shooter.ShooterTargetCommand;
+import frc.team2412.robot.subsystem.Constants;
 import frc.team2412.robot.subsystem.DrivebaseSubsystem;
 import frc.team2412.robot.subsystem.IndexSubsystem;
 import frc.team2412.robot.subsystem.IntakeSubsystem;
@@ -28,8 +29,8 @@ public class JackStealFourBallAutoCommand extends DynamicRequirementSequentialCo
     public static class StealFourBallConstants {
         public static final TrajectoryConstraint[] NORMAL_SPEED = {
                 new FeedforwardConstraint(11.0,
-                        DrivebaseSubsystem.DriveConstants.FEEDFORWARD_CONSTANTS.getVelocityConstant(),
-                        DrivebaseSubsystem.DriveConstants.FEEDFORWARD_CONSTANTS.getAccelerationConstant(), false), // old
+                        Constants.DriveConstants.FEEDFORWARD_CONSTANTS.getVelocityConstant(),
+                        Constants.DriveConstants.FEEDFORWARD_CONSTANTS.getAccelerationConstant(), false), // old
                 // value
                 // was
                 // 11.0
diff --git a/src/main/java/frc/team2412/robot/commands/autonomous/JackStealThreeBallAutoCommand.java b/src/main/java/frc/team2412/robot/commands/autonomous/JackStealThreeBallAutoCommand.java
index 85bec3a5..ad07bf8d 100644
--- a/src/main/java/frc/team2412/robot/commands/autonomous/JackStealThreeBallAutoCommand.java
+++ b/src/main/java/frc/team2412/robot/commands/autonomous/JackStealThreeBallAutoCommand.java
@@ -14,6 +14,7 @@
 import frc.team2412.robot.commands.index.IndexSpitCommand;
 import frc.team2412.robot.commands.intake.IntakeSetInCommand;
 import frc.team2412.robot.commands.shooter.ShooterTargetCommand;
+import frc.team2412.robot.subsystem.Constants;
 import frc.team2412.robot.subsystem.DrivebaseSubsystem;
 import frc.team2412.robot.subsystem.IndexSubsystem;
 import frc.team2412.robot.subsystem.IntakeSubsystem;
@@ -27,8 +28,8 @@ public class JackStealThreeBallAutoCommand extends DynamicRequirementSequentialC
     public static class StealThreeBallConstants {
         public static final TrajectoryConstraint[] NORMAL_SPEED = {
                 new FeedforwardConstraint(11.0,
-                        DrivebaseSubsystem.DriveConstants.FEEDFORWARD_CONSTANTS.getVelocityConstant(),
-                        DrivebaseSubsystem.DriveConstants.FEEDFORWARD_CONSTANTS.getAccelerationConstant(), false), // old
+                        Constants.DriveConstants.FEEDFORWARD_CONSTANTS.getVelocityConstant(),
+                        Constants.DriveConstants.FEEDFORWARD_CONSTANTS.getAccelerationConstant(), false), // old
                 // value
                 // was
                 // 11.0
diff --git a/src/main/java/frc/team2412/robot/commands/autonomous/JackStealThreeBallCompatAutoCommand.java b/src/main/java/frc/team2412/robot/commands/autonomous/JackStealThreeBallCompatAutoCommand.java
index cb9e6f3d..75f3fe1a 100644
--- a/src/main/java/frc/team2412/robot/commands/autonomous/JackStealThreeBallCompatAutoCommand.java
+++ b/src/main/java/frc/team2412/robot/commands/autonomous/JackStealThreeBallCompatAutoCommand.java
@@ -14,6 +14,7 @@
 import frc.team2412.robot.commands.index.IndexSpitCommand;
 import frc.team2412.robot.commands.intake.IntakeSetInCommand;
 import frc.team2412.robot.commands.shooter.ShooterTargetCommand;
+import frc.team2412.robot.subsystem.Constants;
 import frc.team2412.robot.subsystem.DrivebaseSubsystem;
 import frc.team2412.robot.subsystem.IndexSubsystem;
 import frc.team2412.robot.subsystem.IntakeSubsystem;
@@ -28,8 +29,8 @@ public class JackStealThreeBallCompatAutoCommand extends DynamicRequirementSeque
     public static class StealThreeBallConstants {
         public static final TrajectoryConstraint[] NORMAL_SPEED = {
                 new FeedforwardConstraint(11.0,
-                        DrivebaseSubsystem.DriveConstants.FEEDFORWARD_CONSTANTS.getVelocityConstant(),
-                        DrivebaseSubsystem.DriveConstants.FEEDFORWARD_CONSTANTS.getAccelerationConstant(), false), // old
+                        Constants.DriveConstants.FEEDFORWARD_CONSTANTS.getVelocityConstant(),
+                        Constants.DriveConstants.FEEDFORWARD_CONSTANTS.getAccelerationConstant(), false), // old
                 // value
                 // was
                 // 11.0
diff --git a/src/main/java/frc/team2412/robot/commands/autonomous/OneBallAutoCommand.java b/src/main/java/frc/team2412/robot/commands/autonomous/OneBallAutoCommand.java
index c4e1e953..23cde664 100644
--- a/src/main/java/frc/team2412/robot/commands/autonomous/OneBallAutoCommand.java
+++ b/src/main/java/frc/team2412/robot/commands/autonomous/OneBallAutoCommand.java
@@ -17,7 +17,7 @@ public OneBallAutoCommand(IndexSubsystem indexSubsystem, ShooterSubsystem shoote
                 new SimplePathBuilder(Vector2.ZERO, Rotation2.ZERO)
                         .lineTo(new Vector2(0, 70))
                         .build(),
-                DrivebaseSubsystem.DriveConstants.TRAJECTORY_CONSTRAINTS, 0.1);
+                Constants.DriveConstants.TRAJECTORY_CONSTRAINTS, 0.1);
 
         ShooterTargetCommand.TurretManager manager = new ShooterTargetCommand.TurretManager(shooterSubsystem,
                 localizer);
diff --git a/src/main/java/frc/team2412/robot/commands/autonomous/TwoBallAutoCommandLeft.java b/src/main/java/frc/team2412/robot/commands/autonomous/TwoBallAutoCommandLeft.java
index d579acf2..4bda7d6d 100644
--- a/src/main/java/frc/team2412/robot/commands/autonomous/TwoBallAutoCommandLeft.java
+++ b/src/main/java/frc/team2412/robot/commands/autonomous/TwoBallAutoCommandLeft.java
@@ -23,7 +23,7 @@ public TwoBallAutoCommandLeft(IndexSubsystem indexSubsystem, ShooterSubsystem sh
                         .lineTo(new Vector2(429.597, 88.203), Rotation2.fromDegrees(315))
                         .lineTo(new Vector2(426.841, 94.110), Rotation2.fromDegrees(234))
                         .build(),
-                DrivebaseSubsystem.DriveConstants.TRAJECTORY_CONSTRAINTS, 0.1);
+                Constants.DriveConstants.TRAJECTORY_CONSTRAINTS, 0.1);
 
         addCommands(
                 new IntakeSetExtendCommand(intakeSubsystem),
diff --git a/src/main/java/frc/team2412/robot/commands/autonomous/TwoBallAutoCommandMiddle.java b/src/main/java/frc/team2412/robot/commands/autonomous/TwoBallAutoCommandMiddle.java
index 613a6e79..a40599b0 100644
--- a/src/main/java/frc/team2412/robot/commands/autonomous/TwoBallAutoCommandMiddle.java
+++ b/src/main/java/frc/team2412/robot/commands/autonomous/TwoBallAutoCommandMiddle.java
@@ -22,7 +22,7 @@ public TwoBallAutoCommandMiddle(IndexSubsystem indexSubsystem, ShooterSubsystem
                         .lineTo(new Vector2(426.405, 210.657), Rotation2.fromDegrees(25))
                         .lineTo(new Vector2(420, 215), Rotation2.fromDegrees(-65))
                         .build(),
-                DrivebaseSubsystem.DriveConstants.TRAJECTORY_CONSTRAINTS, 0.1);
+                Constants.DriveConstants.TRAJECTORY_CONSTRAINTS, 0.1);
 
         ShooterTargetCommand.TurretManager manager = new ShooterTargetCommand.TurretManager(shooterSubsystem,
                 localizer);
diff --git a/src/main/java/frc/team2412/robot/commands/autonomous/TwoBallAutoCommandRight.java b/src/main/java/frc/team2412/robot/commands/autonomous/TwoBallAutoCommandRight.java
index 1c2495de..0e1a257e 100644
--- a/src/main/java/frc/team2412/robot/commands/autonomous/TwoBallAutoCommandRight.java
+++ b/src/main/java/frc/team2412/robot/commands/autonomous/TwoBallAutoCommandRight.java
@@ -24,7 +24,7 @@ public TwoBallAutoCommandRight(IndexSubsystem indexSubsystem, ShooterSubsystem s
                         .lineTo(new Vector2(337.850, 290.717), Rotation2.fromDegrees(90))
                         .lineTo(new Vector2(337.850, 287), Rotation2.fromDegrees(0))
                         .build(),
-                DrivebaseSubsystem.DriveConstants.TRAJECTORY_CONSTRAINTS, 0.1);
+                Constants.DriveConstants.TRAJECTORY_CONSTRAINTS, 0.1);
 
         addCommands(
                 new IntakeSetExtendCommand(intakeSubsystem),
diff --git a/src/main/java/frc/team2412/robot/commands/autonomous/debug/LinePath.java b/src/main/java/frc/team2412/robot/commands/autonomous/debug/LinePath.java
index b33c8416..16b68974 100644
--- a/src/main/java/frc/team2412/robot/commands/autonomous/debug/LinePath.java
+++ b/src/main/java/frc/team2412/robot/commands/autonomous/debug/LinePath.java
@@ -7,6 +7,7 @@
 
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import frc.team2412.robot.commands.autonomous.Follow2910TrajectoryCommand;
+import frc.team2412.robot.subsystem.Constants;
 import frc.team2412.robot.subsystem.DrivebaseSubsystem;
 
 public class LinePath extends SequentialCommandGroup {
@@ -15,7 +16,7 @@ public LinePath(DrivebaseSubsystem drivebaseSubsystem) {
                 new SimplePathBuilder(new Vector2(0, 0), Rotation2.ZERO)
                         .lineTo(new Vector2(24, 0))
                         .build(),
-                DrivebaseSubsystem.DriveConstants.TRAJECTORY_CONSTRAINTS, 0.1);
+                Constants.DriveConstants.TRAJECTORY_CONSTRAINTS, 0.1);
 
         addCommands(new Follow2910TrajectoryCommand(drivebaseSubsystem, linePathAuto));
     }
diff --git a/src/main/java/frc/team2412/robot/commands/autonomous/debug/SquarePath.java b/src/main/java/frc/team2412/robot/commands/autonomous/debug/SquarePath.java
index 23613953..958f701c 100644
--- a/src/main/java/frc/team2412/robot/commands/autonomous/debug/SquarePath.java
+++ b/src/main/java/frc/team2412/robot/commands/autonomous/debug/SquarePath.java
@@ -7,6 +7,7 @@
 
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import frc.team2412.robot.commands.autonomous.Follow2910TrajectoryCommand;
+import frc.team2412.robot.subsystem.Constants;
 import frc.team2412.robot.subsystem.DrivebaseSubsystem;
 
 public class SquarePath extends SequentialCommandGroup {
@@ -18,7 +19,7 @@ public SquarePath(DrivebaseSubsystem drivebaseSubsystem) {
                         .lineTo(new Vector2(0, 24))
                         .lineTo(new Vector2(0, 0))
                         .build(),
-                DrivebaseSubsystem.DriveConstants.TRAJECTORY_CONSTRAINTS, 0.1);
+                Constants.DriveConstants.TRAJECTORY_CONSTRAINTS, 0.1);
 
         addCommands(new Follow2910TrajectoryCommand(drivebaseSubsystem, squarePathAuto));
     }
diff --git a/src/main/java/frc/team2412/robot/commands/autonomous/debug/StarPath.java b/src/main/java/frc/team2412/robot/commands/autonomous/debug/StarPath.java
index d98412eb..94c58d63 100644
--- a/src/main/java/frc/team2412/robot/commands/autonomous/debug/StarPath.java
+++ b/src/main/java/frc/team2412/robot/commands/autonomous/debug/StarPath.java
@@ -7,6 +7,7 @@
 
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import frc.team2412.robot.commands.autonomous.Follow2910TrajectoryCommand;
+import frc.team2412.robot.subsystem.Constants;
 import frc.team2412.robot.subsystem.DrivebaseSubsystem;
 
 public class StarPath extends SequentialCommandGroup {
@@ -24,7 +25,7 @@ public StarPath(DrivebaseSubsystem drivebaseSubsystem) {
                         .lineTo(new Vector2(18, 18))
                         .lineTo(new Vector2(12, 0))
                         .build(),
-                DrivebaseSubsystem.DriveConstants.TRAJECTORY_CONSTRAINTS, 0.1);
+                Constants.DriveConstants.TRAJECTORY_CONSTRAINTS, 0.1);
 
         addCommands(new Follow2910TrajectoryCommand(drivebaseSubsystem, starPathAuto));
     }
diff --git a/src/main/java/frc/team2412/robot/commands/intake/IntakeSetInCommand.java b/src/main/java/frc/team2412/robot/commands/intake/IntakeSetInCommand.java
index 4be39cf8..ea0c4fb2 100644
--- a/src/main/java/frc/team2412/robot/commands/intake/IntakeSetInCommand.java
+++ b/src/main/java/frc/team2412/robot/commands/intake/IntakeSetInCommand.java
@@ -1,6 +1,6 @@
 package frc.team2412.robot.commands.intake;
 
-import static frc.team2412.robot.subsystem.IntakeSubsystem.IntakeConstants.INTAKE_IN_SPEED;
+import static frc.team2412.robot.subsystem.Constants.IntakeConstants.INTAKE_IN_SPEED;
 
 import frc.team2412.robot.subsystem.IntakeSubsystem;
 
diff --git a/src/main/java/frc/team2412/robot/commands/intake/IntakeSetOutCommand.java b/src/main/java/frc/team2412/robot/commands/intake/IntakeSetOutCommand.java
index 1c6d0c06..6b0b8895 100644
--- a/src/main/java/frc/team2412/robot/commands/intake/IntakeSetOutCommand.java
+++ b/src/main/java/frc/team2412/robot/commands/intake/IntakeSetOutCommand.java
@@ -1,6 +1,6 @@
 package frc.team2412.robot.commands.intake;
 
-import static frc.team2412.robot.subsystem.IntakeSubsystem.IntakeConstants.INTAKE_OUT_SPEED;
+import static frc.team2412.robot.subsystem.Constants.IntakeConstants.INTAKE_OUT_SPEED;
 
 import frc.team2412.robot.subsystem.IntakeSubsystem;
 
diff --git a/src/main/java/frc/team2412/robot/subsystem/DrivebaseSubsystem.java b/src/main/java/frc/team2412/robot/subsystem/DrivebaseSubsystem.java
index 15e53f2b..5eb6360b 100644
--- a/src/main/java/frc/team2412/robot/subsystem/DrivebaseSubsystem.java
+++ b/src/main/java/frc/team2412/robot/subsystem/DrivebaseSubsystem.java
@@ -21,7 +21,6 @@
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.*;
-import edu.wpi.first.wpilibj.shuffleboard.*;
 import edu.wpi.first.wpilibj.smartdashboard.*;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.team2412.robot.*;

From a6eea92b048af1e6b86ef65cb5430e1b631caec3 Mon Sep 17 00:00:00 2001
From: SumedhP <43325361+SumedhP@users.noreply.github.com>
Date: Tue, 6 Sep 2022 16:53:08 +0530
Subject: [PATCH 25/25] Remy removed salt from the sugar box

---
 .../java/frc/team2412/robot/subsystem/ShooterSubsystem.java     | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/frc/team2412/robot/subsystem/ShooterSubsystem.java b/src/main/java/frc/team2412/robot/subsystem/ShooterSubsystem.java
index d609e3b9..774332d6 100644
--- a/src/main/java/frc/team2412/robot/subsystem/ShooterSubsystem.java
+++ b/src/main/java/frc/team2412/robot/subsystem/ShooterSubsystem.java
@@ -473,7 +473,7 @@ public double getTurretAngle() {
      * @param angle
      *            The angle (in degrees) to compare the turret's angle to.
      * @return True if difference between turret angle and given angle is less than
-     *         HOOD_ANGLE_TOLERANCE, False otherwise.
+     *         TURRET_ANGLE_TOLERANCE, False otherwise.
      */
     public boolean isTurretAtAngle(double angle) {
         return Math.abs(getTurretAngle() - angle) < TURRET_ANGLE_TOLERANCE;