IT WORKS!

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());

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();

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.
   }