Balance.java

package frc.robot.commands;

import java.util.Set;

import com.revrobotics.CANSparkMax.IdleMode;

import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Subsystem;
import frc.robot.subsystems.Drive;

public class Balance implements Command {
    private static final double POW_STATIC = .0;
    private static final double POW_SIN_SCALE = .50;
    private static final double NAT_DEG = -1.4; // natural pitch angle of the gyro (the bottom of the robot isn't level)
    private static final double A_THRESH = 1.0;

    private Drive drive;

    public Balance(Drive drive) {
        this.drive = drive;
    }

    @Override
    public void initialize() {
        drive.setIdleMode(IdleMode.kBrake);
    }

    @Override
    public void execute() {
        double a = drive.pitch() - NAT_DEG;
        drive.arcadeDrive(
            Math.abs(a) > A_THRESH ? POW_STATIC + POW_SIN_SCALE * Math.sin(a) : 0.,
            0.
        );
    }

    @Override
    public Set<Subsystem> getRequirements() {
        return Set.of(drive);
    }
}