From 491c5ad30f539799b2565bb8d5025ec9b0a0be94 Mon Sep 17 00:00:00 2001
From: Abhishrek05 <90010729+Abhishrek05@users.noreply.github.com>
Date: Sat, 9 Mar 2024 15:59:53 -0700
Subject: [PATCH] auto balance boiler code (im geekin)

---
 .../robot/commands/Autos/AutoBalance.java     | 49 +++++++++++++++++++
 .../java/frc4388/robot/subsystems/Intake.java |  5 ++
 2 files changed, 54 insertions(+)
 create mode 100644 src/main/java/frc4388/robot/commands/Autos/AutoBalance.java

diff --git a/src/main/java/frc4388/robot/commands/Autos/AutoBalance.java b/src/main/java/frc4388/robot/commands/Autos/AutoBalance.java
new file mode 100644
index 0000000..312325e
--- /dev/null
+++ b/src/main/java/frc4388/robot/commands/Autos/AutoBalance.java
@@ -0,0 +1,49 @@
+// 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.Autos;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.PIDCommand;
+import frc4388.robot.commands.PID;
+import frc4388.robot.subsystems.Intake;
+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 PID {
+  RobotGyro gyro;
+  Intake intake;
+  /** Creates a new AutoBalance. */
+  public AutoBalance(RobotGyro gyro, Intake intake) {
+    super(0.6, 0, 0, 0, 0);
+
+    this.gyro = gyro;
+    this.intake = intake;
+
+    // Use addRequirements() here to declare subsystem dependencies.
+    // Configure additional PID options by calling `getController` here.
+    addRequirements(intake);
+  }
+
+  // Returns true when the command should end.
+
+  public double getError() {
+		var pitch = gyro.getRoll();
+		SmartDashboard.putNumber("pitch", pitch);
+		return pitch;
+	}
+
+	@Override
+	public void runWithOutput(double output) {
+		double out2 = MathUtil.clamp(output / 40, -59, 0);
+		if (Math.abs(getError()) < 3) out2 = 0;
+    intake.talonPIDPosition(out2);
+  }
+
+}
diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java
index e4c0493..986ed14 100644
--- a/src/main/java/frc4388/robot/subsystems/Intake.java
+++ b/src/main/java/frc4388/robot/subsystems/Intake.java
@@ -137,6 +137,11 @@ public class Intake extends SubsystemBase {
     talonPivot.setControl(request.withPosition(-59)); 
   }
 
+  public void talonPIDPosition(double out2) {
+    PositionVoltage request = new PositionVoltage(out2);
+    talonPivot.setControl(request);
+  }
+
   public void talonHandoff() {
     talonIntake.set(-outtakeSpeed.get());
   }