2023WayOfTheRobot/src/main/java/frc4388/robot/commands/AutoBalance.java

// 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.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 {
	RobotGyro gyro;
	SwerveDrive drive;

	/** Creates a new AutoBalanceTF2. */
	public AutoBalance(RobotGyro gyro, SwerveDrive drive) {
		super(.7, .1, 15, 0);
		addRequirements(drive);
	}

	@Override
	public double getError() {
		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(gyro.getPitch()) < 3) out2 = 0;
		drive.drive(out2, 0, 0, false);
	}

	@Override
	public void initialize() {
		super.initialize();
		this.gyro.reset();
	}

	// Returns true when the command should end.
	@Override
	public boolean isFinished() {
		return false;
	}
}