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] 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;