
package com.edinarobotics.zeke.commands;

import edu.wpi.first.wpilibj.command.Command;
import com.edinarobotics.utils.gamepad.Gamepad;
import com.edinarobotics.zeke.subsystems.DrivetrainStrafe;
import com.edinarobotics.zeke.Components;
import com.edinarobotics.utils.gamepad.GamepadAxisState;

public class GamepadDriveStrafeCommand extends Command {
    private Gamepad gamepad;
    private GamepadAxisState gamepadAxisState;
    private DrivetrainStrafe drivetrainStrafe;
    
    public GamepadDriveStrafeCommand(Gamepad gamepad) {
        super("GamepadDriveStrafe");
        this.gamepad = gamepad;
        this.drivetrainStrafe = Components.getInstance().drivetrainStrafe;
        requires(drivetrainStrafe);
    }

    protected void initialize() {
    }

    protected void execute() {
        double magnitude = gamepad.getGamepadAxisState().getLeftMagnitude();
        double direction = gamepad.getGamepadAxisState().getLeftDirection();
        drivetrainStrafe.setMecanumPolarStrafe(magnitude, direction);
    }

    protected boolean isFinished() {
        return false;
    }

    protected void end() {
    }

    protected void interrupted() {
        end();
    }
    
}

@@ -893,16 +893,38 @@
 ction = 
+wpilibAngleCorrection(
 gamepad.
@@ -963,16 +963,17 @@
 ection()
+)
 ;%0A      
@@ -1174,24 +1174,24 @@
 rrupted() %7B%0A
-
         end(
@@ -1204,10 +1204,436 @@
  %7D%0A    %0A
+    public static double wpilibAngleCorrection(double normalAngle)%7B%0A        //Converts normal angle format to the weird WPILib measurement%0A        double convertedAngle = (normalAngle - 90.0) * -1.0;%0A        if(convertedAngle %3E 180.0)%7B%0A            return convertedAngle - 360.0;%0A        %7D%0A        else if(convertedAngle %3C -180.0)%7B%0A            return convertedAngle + 360.0;%0A        %7D%0A        return convertedAngle;%0A    %7D%0A    %0A
 %7D%0A

Add angle correction to normal robot code.