From 07df881b2a5059e809aa265801493c7c928f97c3 Mon Sep 17 00:00:00 2001
From: Abhi <90010729+Abhishrek05@users.noreply.github.com>
Date: Thu, 2 Feb 2023 19:11:10 -0700
Subject: [PATCH] change

---
 .../java/frc4388/robot/RobotContainer.java    |  3 +++
 src/main/java/frc4388/robot/RobotMap.java     |  4 ++--
 .../frc4388/robot/commands/AutoBalance.java   | 21 ++++++++++++-------
 3 files changed, 18 insertions(+), 10 deletions(-)

diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index 6eaf51f..bb7afe4 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -18,6 +18,7 @@ import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import frc4388.robot.Constants.*;
+import frc4388.robot.commands.AutoBalance;
 import frc4388.robot.commands.DriveWithInput;
 import frc4388.robot.subsystems.LED;
 import frc4388.robot.subsystems.SwerveDrive;
@@ -115,6 +116,8 @@ public class RobotContainer {
 
         // new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON)
         //     .onTrue()
+        new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON)
+            .onTrue(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive));
 
         //New interupt button
         new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON)
diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java
index 9679cb3..d4dad2b 100644
--- a/src/main/java/frc4388/robot/RobotMap.java
+++ b/src/main/java/frc4388/robot/RobotMap.java
@@ -23,8 +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);
+  private WPI_Pigeon2 m_pigeon2 = new WPI_Pigeon2(14);
+  public RobotGyro gyro = new RobotGyro(m_pigeon2);
 
 
   public SwerveModule leftFront;
diff --git a/src/main/java/frc4388/robot/commands/AutoBalance.java b/src/main/java/frc4388/robot/commands/AutoBalance.java
index a8cff7d..c0c95f8 100644
--- a/src/main/java/frc4388/robot/commands/AutoBalance.java
+++ b/src/main/java/frc4388/robot/commands/AutoBalance.java
@@ -5,37 +5,42 @@
 package frc4388.robot.commands;
 
 import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc4388.robot.Robot;
+import frc4388.robot.subsystems.SwerveDrive;
+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 AutoBalance extends PelvicInflammatoryDisease {
-	Robot.MicroBot bot;
+	RobotGyro gyro;
+	SwerveDrive drive;
 
 	/** Creates a new AutoBalanceTF2. */
-	public AutoBalance(Robot.MicroBot bot) {
+	public AutoBalance(RobotGyro gyro, SwerveDrive drive) {
 		super(.7, .1, 15, 0);
-		addRequirements(bot);
-		this.bot = bot;
+		addRequirements(drive);
 	}
 
 	@Override
 	public double getError() {
-		return bot.gyro.getPitch();
+		var pitch = gyro.getPitch();
+		SmartDashboard.putNumber("pitch", pitch);
+		return pitch;
 	}
 
 	@Override
 	public void runWithOutput(double output) {
 		double out2 = MathUtil.clamp(output / 40, -.5, .5);
-		if (Math.abs(bot.gyro.getPitch()) < 3) out2 = 0;
-		bot.setOutput(out2);
+		if (Math.abs(gyro.getPitch()) < 3) out2 = 0;
+		drive.drive(out2, 0, 0, false);
 	}
 
 	@Override
 	public void initialize() {
 		super.initialize();
-		this.bot.gyro.reset();
+		this.gyro.reset();
 	}
 
 	// Returns true when the command should end.