From 9d59fdef651b84c26ebf3d2a36e745d42777e3cb Mon Sep 17 00:00:00 2001
From: Astatin3 <mj@mikovsky.com>
Date: Tue, 10 Jan 2023 18:54:58 -0700
Subject: [PATCH 1/8] Update RobotGyro.java

---
 src/main/java/frc4388/utility/RobotGyro.java | 15 ++++++++++++++-
 1 file changed, 14 insertions(+), 1 deletion(-)

diff --git a/src/main/java/frc4388/utility/RobotGyro.java b/src/main/java/frc4388/utility/RobotGyro.java
index ef239ca..2d712c1 100644
--- a/src/main/java/frc4388/utility/RobotGyro.java
+++ b/src/main/java/frc4388/utility/RobotGyro.java
@@ -28,6 +28,11 @@ public class RobotGyro implements Gyro {
     private double m_lastPigeonAngle;
     private double m_deltaPigeonAngle;
 
+    private double diff_Pitch = 0;
+    private double diff_Yaw = 0;
+    private double diff_Roll = 0;
+
+
     /**
      * Creates a Gyro based on a pigeon
      * @param gyro the gyroscope to use for Gyro
@@ -46,6 +51,12 @@ public class RobotGyro implements Gyro {
         m_isGyroAPigeon = false;
     }
 
+    // Sets a 
+    // 
+    public void ResetAng() {
+        
+    }
+
     /**
      * Run in periodic if you are using a pigeon. Updates a delta angle so that it can calculate getRate(). Note
      * that the getRate() method for a navX will likely be much more accurate than for a pigeon.
@@ -100,7 +111,9 @@ public class RobotGyro implements Gyro {
      */
     private double[] getPigeonAngles() {
         double[] angles = new double[3];
-        m_pigeon.getYawPitchRoll(angles);
+        m_pigeon.getPitch(angles);
+        m_pigeon.getYaw(angles);
+        m_pigeon.getRoll(angles);
         return angles;
     }
 

From 01450785940114c4e74198f6691039adc655e9b9 Mon Sep 17 00:00:00 2001
From: Astatin3 <mj@mikovsky.com>
Date: Tue, 10 Jan 2023 18:59:06 -0700
Subject: [PATCH 2/8] Update RobotGyro.java

---
 src/main/java/frc4388/utility/RobotGyro.java | 15 ++++++++-------
 1 file changed, 8 insertions(+), 7 deletions(-)

diff --git a/src/main/java/frc4388/utility/RobotGyro.java b/src/main/java/frc4388/utility/RobotGyro.java
index 2d712c1..54b232d 100644
--- a/src/main/java/frc4388/utility/RobotGyro.java
+++ b/src/main/java/frc4388/utility/RobotGyro.java
@@ -28,8 +28,8 @@ public class RobotGyro implements Gyro {
     private double m_lastPigeonAngle;
     private double m_deltaPigeonAngle;
 
-    private double diff_Pitch = 0;
     private double diff_Yaw = 0;
+    private double diff_Pitch = 0;
     private double diff_Roll = 0;
 
 
@@ -54,7 +54,9 @@ public class RobotGyro implements Gyro {
     // Sets a 
     // 
     public void ResetAng() {
-        
+        diff_Yaw = m_pigeon.getYaw();
+        diff_Pitch = m_pigeon.getPitch();
+        diff_Roll =  m_pigeon.getRoll();
     }
 
     /**
@@ -110,11 +112,10 @@ public class RobotGyro implements Gyro {
 	 *					Roll is within [-90,+90] degrees.
      */
     private double[] getPigeonAngles() {
-        double[] angles = new double[3];
-        m_pigeon.getPitch(angles);
-        m_pigeon.getYaw(angles);
-        m_pigeon.getRoll(angles);
-        return angles;
+        double yaw = m_pigeon.getYaw();
+        double pitch = m_pigeon.getPitch();
+        double roll = m_pigeon.getRoll();
+        return double[(yaw - diff_Yaw), (pitch - diff_Pitch), (roll - diff_Roll)];
     }
 
     @Override

From b9e406bec464d4bb155a5baa0a558a8bf13d95ff Mon Sep 17 00:00:00 2001
From: Ryan Manley <42309874+ryan123rudder@users.noreply.github.com>
Date: Tue, 10 Jan 2023 19:09:27 -0700
Subject: [PATCH 3/8] gyro wrapper class done

---
 src/main/java/frc4388/utility/RobotGyro.java | 30 +++++++++++---------
 1 file changed, 16 insertions(+), 14 deletions(-)

diff --git a/src/main/java/frc4388/utility/RobotGyro.java b/src/main/java/frc4388/utility/RobotGyro.java
index 54b232d..d3761c6 100644
--- a/src/main/java/frc4388/utility/RobotGyro.java
+++ b/src/main/java/frc4388/utility/RobotGyro.java
@@ -28,10 +28,9 @@ public class RobotGyro implements Gyro {
     private double m_lastPigeonAngle;
     private double m_deltaPigeonAngle;
 
-    private double diff_Yaw = 0;
-    private double diff_Pitch = 0;
-    private double diff_Roll = 0;
-
+    private double yawZero = 0;
+    private double pitchZero = 0;
+    private double rollZero = 0;
 
     /**
      * Creates a Gyro based on a pigeon
@@ -51,12 +50,15 @@ public class RobotGyro implements Gyro {
         m_isGyroAPigeon = false;
     }
 
-    // Sets a 
-    // 
-    public void ResetAng() {
-        diff_Yaw = m_pigeon.getYaw();
-        diff_Pitch = m_pigeon.getPitch();
-        diff_Roll =  m_pigeon.getRoll();
+    /**
+     * Resets yaw, pitch, and roll.
+     */
+    public void resetAllAngles() {
+        if (!m_isGyroAPigeon) return;
+
+        yawZero = m_pigeon.getYaw();
+        pitchZero = m_pigeon.getPitch();
+        rollZero =  m_pigeon.getRoll();
     }
 
     /**
@@ -112,10 +114,10 @@ public class RobotGyro implements Gyro {
 	 *					Roll is within [-90,+90] degrees.
      */
     private double[] getPigeonAngles() {
-        double yaw = m_pigeon.getYaw();
-        double pitch = m_pigeon.getPitch();
-        double roll = m_pigeon.getRoll();
-        return double[(yaw - diff_Yaw), (pitch - diff_Pitch), (roll - diff_Roll)];
+        double[] ypr = new double[3];
+        m_pigeon.getYawPitchRoll(ypr);
+
+        return new double[] {(ypr[0] - yawZero), (ypr[1] - pitchZero), (ypr[2] - rollZero)};
     }
 
     @Override

From e89116d82e09adb17010985b3ae6db88b422bee6 Mon Sep 17 00:00:00 2001
From: Astatin3 <mj@mikovsky.com>
Date: Thu, 12 Jan 2023 18:49:21 -0700
Subject: [PATCH 4/8] added pid controller

---
 src/main/java/frc4388/robot/Robot.java        | 10 +++++-
 .../java/frc4388/robot/RobotContainer.java    |  3 ++
 src/main/java/frc4388/robot/RobotMap.java     |  3 ++
 .../robot/commands/AutoBalanceTF2.java        | 36 +++++++++++++++++++
 src/main/java/frc4388/utility/RobotGyro.java  | 17 ++++-----
 5 files changed, 60 insertions(+), 9 deletions(-)
 create mode 100644 src/main/java/frc4388/robot/commands/AutoBalanceTF2.java

diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java
index e145b38..8db637a 100644
--- a/src/main/java/frc4388/robot/Robot.java
+++ b/src/main/java/frc4388/robot/Robot.java
@@ -7,9 +7,13 @@
 
 package frc4388.robot;
 
+import com.ctre.phoenix.sensors.WPI_Pigeon2;
+
 import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import frc4388.utility.RobotGyro;
 import frc4388.utility.RobotTime;
 
 /**
@@ -106,6 +110,8 @@ public class Robot extends TimedRobot {
       m_autonomousCommand.cancel();
     }
     m_robotTime.startMatchTime();
+
+    m_robotContainer.gyroRef.reset();
   }
 
   /**
@@ -113,7 +119,9 @@ public class Robot extends TimedRobot {
    */
   @Override
   public void teleopPeriodic() {
-    
+    SmartDashboard.putNumber("yaw", m_robotContainer.gyroRef.getAngle());
+    SmartDashboard.putNumber("pitch", m_robotContainer.gyroRef.getPitch());
+    SmartDashboard.putNumber("roll", m_robotContainer.gyroRef.getRoll());
   }
 
   /**
diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index 99f5cb1..16c43d3 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -15,6 +15,7 @@ import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import frc4388.robot.Constants.*;
 import frc4388.robot.subsystems.LED;
 import frc4388.utility.LEDPatterns;
+import frc4388.utility.RobotGyro;
 import frc4388.utility.controller.IHandController;
 import frc4388.utility.controller.XboxController;
 
@@ -36,6 +37,8 @@ public class RobotContainer {
     private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID);
     private final XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID);
 
+    public RobotGyro gyroRef = m_robotMap.gyro;
+
     /**
      * The container for the robot. Contains subsystems, OI devices, and commands.
      */
diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java
index 5f17853..23312a8 100644
--- a/src/main/java/frc4388/robot/RobotMap.java
+++ b/src/main/java/frc4388/robot/RobotMap.java
@@ -11,6 +11,7 @@ import com.ctre.phoenix.motorcontrol.InvertType;
 import com.ctre.phoenix.motorcontrol.NeutralMode;
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
 import com.ctre.phoenix.sensors.PigeonIMU;
+import com.ctre.phoenix.sensors.WPI_Pigeon2;
 
 import edu.wpi.first.wpilibj.motorcontrol.Spark;
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
@@ -22,6 +23,8 @@ import frc4388.utility.RobotGyro;
  * testing and modularization.
  */
 public class RobotMap {
+    private WPI_Pigeon2 m_pigeon2 = new WPI_Pigeon2(14);
+    public RobotGyro gyro = new RobotGyro(m_pigeon2);
 
     public RobotMap() {
         configureLEDMotorControllers();
diff --git a/src/main/java/frc4388/robot/commands/AutoBalanceTF2.java b/src/main/java/frc4388/robot/commands/AutoBalanceTF2.java
new file mode 100644
index 0000000..33a672e
--- /dev/null
+++ b/src/main/java/frc4388/robot/commands/AutoBalanceTF2.java
@@ -0,0 +1,36 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc4388.robot.commands;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.wpilibj2.command.PIDCommand;
+
+// NOTE:  Consider using this command inline, rather than writing a subclass.  For more
+// information, see:
+// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
+public class AutoBalanceTF2 extends PIDCommand {
+  /** Creates a new AutoBalanceTF2. */
+  public AutoBalanceTF2() {
+    super(
+        // The controller that the command will use
+        new PIDController(0, 0, 0),
+        // This should return the measurement
+        () -> 0,
+        // This should return the setpoint (can also be a constant)
+        () -> 0,
+        // This uses the output
+        output -> {
+          // Use the output here
+        });
+    // Use addRequirements() here to declare subsystem dependencies.
+    // Configure additional PID options by calling `getController` here.
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}
diff --git a/src/main/java/frc4388/utility/RobotGyro.java b/src/main/java/frc4388/utility/RobotGyro.java
index d3761c6..eb8621c 100644
--- a/src/main/java/frc4388/utility/RobotGyro.java
+++ b/src/main/java/frc4388/utility/RobotGyro.java
@@ -8,6 +8,7 @@
 package frc4388.utility;
 
 import com.ctre.phoenix.sensors.PigeonIMU;
+import com.ctre.phoenix.sensors.WPI_Pigeon2;
 import com.ctre.phoenix.sensors.PigeonIMU.CalibrationMode;
 import com.kauailabs.navx.frc.AHRS;
 
@@ -21,14 +22,13 @@ import edu.wpi.first.math.MathUtil;
 public class RobotGyro implements Gyro {
     private RobotTime m_robotTime = RobotTime.getInstance();
 
-    private PigeonIMU m_pigeon = null;
+    private WPI_Pigeon2 m_pigeon = null;
     private AHRS m_navX = null;
     public boolean m_isGyroAPigeon; //true if pigeon, false if navX
 
     private double m_lastPigeonAngle;
     private double m_deltaPigeonAngle;
 
-    private double yawZero = 0;
     private double pitchZero = 0;
     private double rollZero = 0;
 
@@ -36,7 +36,7 @@ public class RobotGyro implements Gyro {
      * Creates a Gyro based on a pigeon
      * @param gyro the gyroscope to use for Gyro
      */
-    public RobotGyro(PigeonIMU gyro) {
+    public RobotGyro(WPI_Pigeon2 gyro) {
         m_pigeon = gyro;
         m_isGyroAPigeon = true;
     }
@@ -53,10 +53,9 @@ public class RobotGyro implements Gyro {
     /**
      * Resets yaw, pitch, and roll.
      */
-    public void resetAllAngles() {
+    public void resetZeroValues() {
         if (!m_isGyroAPigeon) return;
 
-        yawZero = m_pigeon.getYaw();
         pitchZero = m_pigeon.getPitch();
         rollZero =  m_pigeon.getRoll();
     }
@@ -90,7 +89,7 @@ public class RobotGyro implements Gyro {
     @Override
     public void calibrate() {
         if (m_isGyroAPigeon) {
-            m_pigeon.enterCalibrationMode(CalibrationMode.Temperature);
+            m_pigeon.calibrate();
         } else {
             m_navX.calibrate();
         }
@@ -98,6 +97,8 @@ public class RobotGyro implements Gyro {
 
     @Override
     public void reset() {
+        resetZeroValues();
+
         if (m_isGyroAPigeon) {
             m_pigeon.setYaw(0);
         } else {
@@ -117,7 +118,7 @@ public class RobotGyro implements Gyro {
         double[] ypr = new double[3];
         m_pigeon.getYawPitchRoll(ypr);
 
-        return new double[] {(ypr[0] - yawZero), (ypr[1] - pitchZero), (ypr[2] - rollZero)};
+        return new double[] {ypr[0], (ypr[1] - pitchZero), (ypr[2] - rollZero)};
     }
 
     @Override
@@ -182,7 +183,7 @@ public class RobotGyro implements Gyro {
         }
     }
 
-    public PigeonIMU getPigeon(){
+    public WPI_Pigeon2 getPigeon(){
         return m_pigeon;
     }
 

From 2c5aa4b3532c8bfb38a3455d6c13f8be2166343a Mon Sep 17 00:00:00 2001
From: Ryan Manley <42309874+ryan123rudder@users.noreply.github.com>
Date: Thu, 12 Jan 2023 18:51:28 -0700
Subject: [PATCH 5/8] Commit moment

---
 src/main/java/frc4388/robot/Robot.java       | 11 ++++++++++-
 src/main/java/frc4388/utility/RobotGyro.java | 15 +++++++++------
 2 files changed, 19 insertions(+), 7 deletions(-)

diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java
index e145b38..2eee91d 100644
--- a/src/main/java/frc4388/robot/Robot.java
+++ b/src/main/java/frc4388/robot/Robot.java
@@ -7,9 +7,13 @@
 
 package frc4388.robot;
 
+import com.ctre.phoenix.sensors.WPI_Pigeon2;
+
 import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import frc4388.utility.RobotGyro;
 import frc4388.utility.RobotTime;
 
 /**
@@ -25,6 +29,9 @@ public class Robot extends TimedRobot {
   private RobotTime m_robotTime = RobotTime.getInstance();
   private RobotContainer m_robotContainer;
 
+  private WPI_Pigeon2 pigeon = new WPI_Pigeon2(14);
+  private RobotGyro gyro = new RobotGyro(pigeon);
+
   /**
    * This function is run when the robot is first started up and should be
    * used for any initialization code.
@@ -113,7 +120,9 @@ public class Robot extends TimedRobot {
    */
   @Override
   public void teleopPeriodic() {
-    
+    SmartDashboard.putNumber("Gyro Yaw", gyro.getYaw());
+    SmartDashboard.putNumber("Gyro Pitch", gyro.getPitch());
+    SmartDashboard.putNumber("Gyro Roll", gyro.getRoll());
   }
 
   /**
diff --git a/src/main/java/frc4388/utility/RobotGyro.java b/src/main/java/frc4388/utility/RobotGyro.java
index d3761c6..669122b 100644
--- a/src/main/java/frc4388/utility/RobotGyro.java
+++ b/src/main/java/frc4388/utility/RobotGyro.java
@@ -7,8 +7,7 @@
 
 package frc4388.utility;
 
-import com.ctre.phoenix.sensors.PigeonIMU;
-import com.ctre.phoenix.sensors.PigeonIMU.CalibrationMode;
+import com.ctre.phoenix.sensors.WPI_Pigeon2;
 import com.kauailabs.navx.frc.AHRS;
 
 // import edu.wpi.first.wpilibj.GyroBase;
@@ -21,7 +20,7 @@ import edu.wpi.first.math.MathUtil;
 public class RobotGyro implements Gyro {
     private RobotTime m_robotTime = RobotTime.getInstance();
 
-    private PigeonIMU m_pigeon = null;
+    private WPI_Pigeon2 m_pigeon = null;
     private AHRS m_navX = null;
     public boolean m_isGyroAPigeon; //true if pigeon, false if navX
 
@@ -36,7 +35,7 @@ public class RobotGyro implements Gyro {
      * Creates a Gyro based on a pigeon
      * @param gyro the gyroscope to use for Gyro
      */
-    public RobotGyro(PigeonIMU gyro) {
+    public RobotGyro(WPI_Pigeon2 gyro) {
         m_pigeon = gyro;
         m_isGyroAPigeon = true;
     }
@@ -90,7 +89,7 @@ public class RobotGyro implements Gyro {
     @Override
     public void calibrate() {
         if (m_isGyroAPigeon) {
-            m_pigeon.enterCalibrationMode(CalibrationMode.Temperature);
+            m_pigeon.calibrate();
         } else {
             m_navX.calibrate();
         }
@@ -129,6 +128,10 @@ public class RobotGyro implements Gyro {
         }
     }
 
+    public double getYaw() {
+       return this.getAngle();
+    }
+
     /**
      * Gets an absolute heading of the robot
      * @return heading from -180 to 180 degrees
@@ -182,7 +185,7 @@ public class RobotGyro implements Gyro {
         }
     }
 
-    public PigeonIMU getPigeon(){
+    public WPI_Pigeon2 getPigeon(){
         return m_pigeon;
     }
 

From da6ccb19d1f950086d7628445627a110a66e2bfc Mon Sep 17 00:00:00 2001
From: Ryan Manley <42309874+ryan123rudder@users.noreply.github.com>
Date: Fri, 13 Jan 2023 20:24:38 -0700
Subject: [PATCH 6/8] IT WORKS!

---
 src/main/java/frc4388/robot/Robot.java        | 40 ++++++++++++++++---
 src/main/java/frc4388/robot/RobotMap.java     |  3 ++
 .../robot/commands/AutoBalanceTF2.java        | 31 ++++++++++++--
 3 files changed, 66 insertions(+), 8 deletions(-)

diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java
index c86560c..c6f72b1 100644
--- a/src/main/java/frc4388/robot/Robot.java
+++ b/src/main/java/frc4388/robot/Robot.java
@@ -7,12 +7,16 @@
 
 package frc4388.robot;
 
+import com.ctre.phoenix.motorcontrol.ControlMode;
+import com.ctre.phoenix.motorcontrol.can.TalonSRX;
 import com.ctre.phoenix.sensors.WPI_Pigeon2;
 
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc4388.robot.commands.AutoBalanceTF2;
 import frc4388.utility.RobotGyro;
 import frc4388.utility.RobotTime;
 
@@ -29,8 +33,34 @@ public class Robot extends TimedRobot {
   private RobotTime m_robotTime = RobotTime.getInstance();
   private RobotContainer m_robotContainer;
 
-  private WPI_Pigeon2 pigeon = new WPI_Pigeon2(14);
-  private RobotGyro gyro = new RobotGyro(pigeon);
+  public static class MicroBot extends SubsystemBase {
+    public WPI_Pigeon2 pigeon = new WPI_Pigeon2(14);
+    public RobotGyro gyro = new RobotGyro(pigeon);
+
+    public TalonSRX frontLeft = new TalonSRX(2);
+    public TalonSRX backLeft = new TalonSRX(3);
+    public TalonSRX backRight = new TalonSRX(5);
+    public TalonSRX frontRight = new TalonSRX(4);
+
+    public MicroBot() {
+      frontRight.configFactoryDefault();
+      frontLeft.configFactoryDefault();
+      backLeft.configFactoryDefault();
+      backRight.configFactoryDefault();
+  
+      frontLeft.setInverted(true);
+      backLeft.setInverted(true);
+    }
+
+    public void setOutput(double output) {
+      frontRight.set(ControlMode.PercentOutput, output);
+      frontLeft.set(ControlMode.PercentOutput, output);
+      backLeft.set(ControlMode.PercentOutput, output);
+      backRight.set(ControlMode.PercentOutput, output);
+    }
+  }
+
+  private MicroBot bot = new MicroBot();
 
   /**
    * This function is run when the robot is first started up and should be
@@ -41,6 +71,9 @@ public class Robot extends TimedRobot {
     // Instantiate our RobotContainer.  This will perform all our button bindings, and put our
     // autonomous chooser on the dashboard.
     m_robotContainer = new RobotContainer();
+
+    bot.setDefaultCommand(new AutoBalanceTF2(bot.frontLeft,
+      bot.frontRight, bot.backLeft, bot.backRight, bot.gyro, bot));
   }
 
   /**
@@ -122,9 +155,6 @@ public class Robot extends TimedRobot {
    */
   @Override
   public void teleopPeriodic() {
-    SmartDashboard.putNumber("Gyro Yaw", gyro.getYaw());
-    SmartDashboard.putNumber("Gyro Pitch", gyro.getPitch());
-    SmartDashboard.putNumber("Gyro Roll", gyro.getRoll());
     SmartDashboard.putNumber("yaw", m_robotContainer.gyroRef.getAngle());
     SmartDashboard.putNumber("pitch", m_robotContainer.gyroRef.getPitch());
     SmartDashboard.putNumber("roll", m_robotContainer.gyroRef.getRoll());
diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java
index 23312a8..a7c4517 100644
--- a/src/main/java/frc4388/robot/RobotMap.java
+++ b/src/main/java/frc4388/robot/RobotMap.java
@@ -9,6 +9,7 @@ package frc4388.robot;
 
 import com.ctre.phoenix.motorcontrol.InvertType;
 import com.ctre.phoenix.motorcontrol.NeutralMode;
+import com.ctre.phoenix.motorcontrol.can.TalonSRX;
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
 import com.ctre.phoenix.sensors.PigeonIMU;
 import com.ctre.phoenix.sensors.WPI_Pigeon2;
@@ -26,6 +27,8 @@ public class RobotMap {
     private WPI_Pigeon2 m_pigeon2 = new WPI_Pigeon2(14);
     public RobotGyro gyro = new RobotGyro(m_pigeon2);
 
+    private TalonSRX backleft = new TalonSRX(-1);
+
     public RobotMap() {
         configureLEDMotorControllers();
         configureDriveMotorControllers();
diff --git a/src/main/java/frc4388/robot/commands/AutoBalanceTF2.java b/src/main/java/frc4388/robot/commands/AutoBalanceTF2.java
index 33a672e..4299737 100644
--- a/src/main/java/frc4388/robot/commands/AutoBalanceTF2.java
+++ b/src/main/java/frc4388/robot/commands/AutoBalanceTF2.java
@@ -4,26 +4,51 @@
 
 package frc4388.robot.commands;
 
+import com.ctre.phoenix.motorcontrol.ControlMode;
+import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
+import com.ctre.phoenix.motorcontrol.can.TalonSRX;
+import com.ctre.phoenix.sensors.WPI_Pigeon2;
+
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.PIDCommand;
+import frc4388.robot.Robot;
+import frc4388.utility.RobotGyro;
 
 // NOTE:  Consider using this command inline, rather than writing a subclass.  For more
 // information, see:
 // https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
 public class AutoBalanceTF2 extends PIDCommand {
+  // private TalonSRX frontLeft;
+  // private TalonSRX frontRight;
+  // private TalonSRX backLeft;
+  // private TalonSRX backRight;
+
   /** Creates a new AutoBalanceTF2. */
-  public AutoBalanceTF2() {
+  public AutoBalanceTF2(TalonSRX frontLeft, TalonSRX frontRight, TalonSRX backLeft, TalonSRX backRight, RobotGyro gyro, Robot.MicroBot bot) {
     super(
         // The controller that the command will use
-        new PIDController(0, 0, 0),
+        new PIDController(.7, .02, .1),
         // This should return the measurement
-        () -> 0,
+        () -> Math.abs(gyro.getPitch()) < 3 ? 0 : gyro.getPitch(),
         // This should return the setpoint (can also be a constant)
         () -> 0,
         // This uses the output
         output -> {
           // Use the output here
+          double out2 = -MathUtil.clamp(output / 20, -1, 1);
+          if (Math.abs(gyro.getPitch()) < 3) out2 = 0;
+
+          SmartDashboard.putNumber("Output", out2);
+          frontLeft.set(ControlMode.PercentOutput, out2);
+          frontRight.set(ControlMode.PercentOutput, out2);
+          backRight.set(ControlMode.PercentOutput, out2);
+          backLeft.set(ControlMode.PercentOutput, out2);
         });
+
+        gyro.reset();
+        addRequirements(bot);
     // Use addRequirements() here to declare subsystem dependencies.
     // Configure additional PID options by calling `getController` here.
   }

From c454a08eb14ba6d0fe646fd1a24f71b972a3ee58 Mon Sep 17 00:00:00 2001
From: Ryan Manley <42309874+ryan123rudder@users.noreply.github.com>
Date: Sat, 14 Jan 2023 10:37:09 -0700
Subject: [PATCH 7/8] Added PelvicInflamatoryDisease

---
 src/main/java/frc4388/robot/Robot.java        |  3 +-
 .../robot/commands/AutoBalanceTF2.java        | 61 ++++++---------
 .../commands/PelvicInflamitoryDisease.java    | 59 ++++++++++++++
 src/main/java/frc4388/utility/Gains.java      | 77 +++++++++++++++++++
 4 files changed, 162 insertions(+), 38 deletions(-)
 create mode 100644 src/main/java/frc4388/robot/commands/PelvicInflamitoryDisease.java
 create mode 100644 src/main/java/frc4388/utility/Gains.java

diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java
index c6f72b1..7e548a1 100644
--- a/src/main/java/frc4388/robot/Robot.java
+++ b/src/main/java/frc4388/robot/Robot.java
@@ -72,8 +72,7 @@ public class Robot extends TimedRobot {
     // autonomous chooser on the dashboard.
     m_robotContainer = new RobotContainer();
 
-    bot.setDefaultCommand(new AutoBalanceTF2(bot.frontLeft,
-      bot.frontRight, bot.backLeft, bot.backRight, bot.gyro, bot));
+    bot.setDefaultCommand(new AutoBalanceTF2(bot));
   }
 
   /**
diff --git a/src/main/java/frc4388/robot/commands/AutoBalanceTF2.java b/src/main/java/frc4388/robot/commands/AutoBalanceTF2.java
index 4299737..1c6f63c 100644
--- a/src/main/java/frc4388/robot/commands/AutoBalanceTF2.java
+++ b/src/main/java/frc4388/robot/commands/AutoBalanceTF2.java
@@ -16,46 +16,35 @@ import edu.wpi.first.wpilibj2.command.PIDCommand;
 import frc4388.robot.Robot;
 import frc4388.utility.RobotGyro;
 
-// NOTE:  Consider using this command inline, rather than writing a subclass.  For more
+// NOTE:	Consider using this command inline, rather than writing a subclass.	For more
 // information, see:
 // https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
-public class AutoBalanceTF2 extends PIDCommand {
-  // private TalonSRX frontLeft;
-  // private TalonSRX frontRight;
-  // private TalonSRX backLeft;
-  // private TalonSRX backRight;
+public class AutoBalanceTF2 extends PelvicInflamitoryDisease {
+	Robot.MicroBot bot;
 
-  /** Creates a new AutoBalanceTF2. */
-  public AutoBalanceTF2(TalonSRX frontLeft, TalonSRX frontRight, TalonSRX backLeft, TalonSRX backRight, RobotGyro gyro, Robot.MicroBot bot) {
-    super(
-        // The controller that the command will use
-        new PIDController(.7, .02, .1),
-        // This should return the measurement
-        () -> Math.abs(gyro.getPitch()) < 3 ? 0 : gyro.getPitch(),
-        // This should return the setpoint (can also be a constant)
-        () -> 0,
-        // This uses the output
-        output -> {
-          // Use the output here
-          double out2 = -MathUtil.clamp(output / 20, -1, 1);
-          if (Math.abs(gyro.getPitch()) < 3) out2 = 0;
+	/** Creates a new AutoBalanceTF2. */
+	// ! finish integrating PelvicInflamatoryDisease
+	public AutoBalanceTF2(Robot.MicroBot bot) {
+		super(.7, .02, .1, 0);
+		addRequirements(bot);
+		this.bot = bot;
+	}
 
-          SmartDashboard.putNumber("Output", out2);
-          frontLeft.set(ControlMode.PercentOutput, out2);
-          frontRight.set(ControlMode.PercentOutput, out2);
-          backRight.set(ControlMode.PercentOutput, out2);
-          backLeft.set(ControlMode.PercentOutput, out2);
-        });
+	@Override
+	public double getError() {
+		return bot.gyro.getPitch();
+	}
 
-        gyro.reset();
-        addRequirements(bot);
-    // Use addRequirements() here to declare subsystem dependencies.
-    // Configure additional PID options by calling `getController` here.
-  }
+	@Override
+	public void runWithOutput(double output) {
+		double out2 = -MathUtil.clamp(output / 20, -1, 1);
+		if (Math.abs(bot.gyro.getPitch()) < 3) out2 = 0;
+		bot.setOutput(out2);
+	}
 
-  // Returns true when the command should end.
-  @Override
-  public boolean isFinished() {
-    return false;
-  }
+	// Returns true when the command should end.
+	@Override
+	public boolean isFinished() {
+		return false;
+	}
 }
diff --git a/src/main/java/frc4388/robot/commands/PelvicInflamitoryDisease.java b/src/main/java/frc4388/robot/commands/PelvicInflamitoryDisease.java
new file mode 100644
index 0000000..c596201
--- /dev/null
+++ b/src/main/java/frc4388/robot/commands/PelvicInflamitoryDisease.java
@@ -0,0 +1,59 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc4388.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.utility.Gains;
+
+public abstract class PelvicInflamitoryDisease extends CommandBase {
+	protected Gains gains;
+	private double output = 0;
+
+	/** Creates a new PelvicInflamitoryDisease. */
+	public PelvicInflamitoryDisease(double kp, double ki, double kd, double kf) {
+		gains = new Gains(kp, ki, kd, kf, 0);
+	}
+
+	public PelvicInflamitoryDisease(Gains gains) {
+		this.gains = gains;
+	}
+
+	/** produces the error from the setpoint */
+	public abstract double getError();
+	/** figure it out bitch */
+	public abstract void runWithOutput(double output);
+
+	// Called when the command is initially scheduled.
+	@Override
+	public void initialize() {
+		output = 0;
+	}
+
+	private double prevError, cumError = 0;
+	// Called every time the scheduler runs while the command is scheduled.
+	@Override
+	public void execute() {
+		double error = getError();
+		cumError += error;
+		double delta = error - prevError;
+
+		output = error * gains.kP;
+		output += cumError * gains.kI;
+		output += delta * gains.kD;
+		output += gains.kF;
+
+		runWithOutput(output);
+	}
+
+	// Called once the command ends or is interrupted.
+	@Override
+	public void end(boolean interrupted) {}
+
+	// Returns true when the command should end.
+	@Override
+	public boolean isFinished() {
+		return false;
+	}
+}
diff --git a/src/main/java/frc4388/utility/Gains.java b/src/main/java/frc4388/utility/Gains.java
new file mode 100644
index 0000000..7cda1e0
--- /dev/null
+++ b/src/main/java/frc4388/utility/Gains.java
@@ -0,0 +1,77 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc4388.utility;
+
+/** Add your docs here. */
+public class Gains {
+    public double kP;
+    public double kI;
+    public double kD;
+    public double kF;
+    public int kIZone;
+    public double kPeakOutput;
+    public double kMaxOutput;
+    public double kMinOutput;
+
+    /**
+     * Creates Gains object for PIDs
+     * @param kP The P value.
+     * @param kI The I value.
+     * @param kD The D value.
+     * @param kF The F value.
+     * @param kIZone The zone of the I value.
+     * @param kPeakOutput The peak output setting the motors to run the gains at, in both forward and reverse directions. By default 1.0.
+     */
+    public Gains(double kP, double kI, double kD, double kF, int kIZone, double kPeakOutput) {
+        this.kP = kP;
+        this.kI = kI;
+        this.kD = kD;
+        this.kF = kF;
+        this.kIZone = kIZone;
+        this.kPeakOutput = kPeakOutput;
+        this.kMaxOutput = kPeakOutput;
+        this.kMinOutput = -kPeakOutput;
+    }
+
+    /**
+     * Creates Gains object for PIDs
+     * @param kP The P value.
+     * @param kI The I value.
+     * @param kD The D value.
+     * @param kF The F value.
+     * @param kIZone The zone of the I value.
+     */
+    public Gains(double kP, double kI, double kD, double kF, int kIZone) {
+        this.kP = kP;
+        this.kI = kI;
+        this.kD = kD;
+        this.kF = kF;
+        this.kIZone = kIZone;
+        this.kPeakOutput = 1.0;
+        this.kMaxOutput = 1.0;
+        this.kMinOutput = -1.0;
+    }
+
+    /**
+     * Creates Gains object for PIDs
+     * @param kP The P value.
+     * @param kI The I value.
+     * @param kD The D value.
+     * @param kF The F value.
+     * @param kIZone The zone of the I value.
+     * @param kMinOutput The lowest output setting to run the gains at, usually in the reverse direction. By default -1.0.
+     * @param kMaxOutput The highest output setting to run the gains at, usually in the forward direction. By default 1.0.
+     */
+    public Gains(double kP, double kI, double kD, double kF, int kIZone, double kMaxOutput, double kMinOutput) {
+        this.kP = kP;
+        this.kI = kI;
+        this.kD = kD;
+        this.kF = kF;
+        this.kIZone = kIZone;
+        this.kMaxOutput = kMaxOutput;
+        this.kMinOutput = kMinOutput;
+        this.kPeakOutput = (Math.abs(kMinOutput) > Math.abs(kMaxOutput)) ? Math.abs(kMinOutput) : Math.abs(kMaxOutput);
+    }    
+}
\ No newline at end of file

From 158513c92ddaa988721509806fef58e9c4bb7d29 Mon Sep 17 00:00:00 2001
From: Ryan Manley <42309874+ryan123rudder@users.noreply.github.com>
Date: Sat, 14 Jan 2023 14:26:17 -0700
Subject: [PATCH 8/8] more worky

---
 .../robot/commands/AutoBalanceTF2.java        | 20 ++++++++-----------
 .../commands/PelvicInflamitoryDisease.java    |  2 +-
 2 files changed, 9 insertions(+), 13 deletions(-)

diff --git a/src/main/java/frc4388/robot/commands/AutoBalanceTF2.java b/src/main/java/frc4388/robot/commands/AutoBalanceTF2.java
index 1c6f63c..d06631c 100644
--- a/src/main/java/frc4388/robot/commands/AutoBalanceTF2.java
+++ b/src/main/java/frc4388/robot/commands/AutoBalanceTF2.java
@@ -4,17 +4,8 @@
 
 package frc4388.robot.commands;
 
-import com.ctre.phoenix.motorcontrol.ControlMode;
-import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
-import com.ctre.phoenix.motorcontrol.can.TalonSRX;
-import com.ctre.phoenix.sensors.WPI_Pigeon2;
-
 import edu.wpi.first.math.MathUtil;
-import edu.wpi.first.math.controller.PIDController;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj2.command.PIDCommand;
 import frc4388.robot.Robot;
-import frc4388.utility.RobotGyro;
 
 // NOTE:	Consider using this command inline, rather than writing a subclass.	For more
 // information, see:
@@ -23,9 +14,8 @@ public class AutoBalanceTF2 extends PelvicInflamitoryDisease {
 	Robot.MicroBot bot;
 
 	/** Creates a new AutoBalanceTF2. */
-	// ! finish integrating PelvicInflamatoryDisease
 	public AutoBalanceTF2(Robot.MicroBot bot) {
-		super(.7, .02, .1, 0);
+		super(.7, .1, 15, 0);
 		addRequirements(bot);
 		this.bot = bot;
 	}
@@ -37,11 +27,17 @@ public class AutoBalanceTF2 extends PelvicInflamitoryDisease {
 
 	@Override
 	public void runWithOutput(double output) {
-		double out2 = -MathUtil.clamp(output / 20, -1, 1);
+		double out2 = MathUtil.clamp(output / 40, -.5, .5);
 		if (Math.abs(bot.gyro.getPitch()) < 3) out2 = 0;
 		bot.setOutput(out2);
 	}
 
+	@Override
+	public void initialize() {
+		super.initialize();
+		this.bot.gyro.reset();
+	}
+
 	// Returns true when the command should end.
 	@Override
 	public boolean isFinished() {
diff --git a/src/main/java/frc4388/robot/commands/PelvicInflamitoryDisease.java b/src/main/java/frc4388/robot/commands/PelvicInflamitoryDisease.java
index c596201..e377c0a 100644
--- a/src/main/java/frc4388/robot/commands/PelvicInflamitoryDisease.java
+++ b/src/main/java/frc4388/robot/commands/PelvicInflamitoryDisease.java
@@ -36,7 +36,7 @@ public abstract class PelvicInflamitoryDisease extends CommandBase {
 	@Override
 	public void execute() {
 		double error = getError();
-		cumError += error;
+		cumError += error * .02; // 20 ms
 		double delta = error - prevError;
 
 		output = error * gains.kP;