diff --git a/src/main/java/frc4388/robot/commands/DriveWithJoystick.java b/src/main/java/frc4388/robot/commands/DriveWithJoystick.java index 2811fe1..01375a4 100644 --- a/src/main/java/frc4388/robot/commands/DriveWithJoystick.java +++ b/src/main/java/frc4388/robot/commands/DriveWithJoystick.java @@ -7,6 +7,7 @@ package frc4388.robot.commands; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.subsystems.Drive; import frc4388.utility.controller.IHandController; @@ -42,22 +43,23 @@ public class DriveWithJoystick extends CommandBase { double steerInput = -m_controller.getLeftYAxis(); double moveOutput = 0; double steerOutput = 0; - if (moveInput >= 0){ - moveOutput = -Math.cos(1.571*moveInput)+1; + if (steerInput >= 0){ + steerOutput = -Math.cos(1.571*steerInput)+1; } else { - moveOutput = Math.cos(1.571*moveInput)-1; + steerOutput = Math.cos(1.571*steerInput)-1; } - double cosMultiplier = .55; + double cosMultiplier = 1.0; double deadzone = .1; - if (steerInput > 0){ - steerOutput = -(cosMultiplier - deadzone) * Math.cos(1.571*steerInput) + cosMultiplier; - } else if (steerInput < 0) { - steerOutput = (cosMultiplier - deadzone) * Math.cos(1.571*steerInput) - cosMultiplier; + if (moveInput > 0){ + moveOutput = -(cosMultiplier - deadzone) * Math.cos(1.571*moveInput) + cosMultiplier; + } else if (moveInput < 0) { + moveOutput = (cosMultiplier - deadzone) * Math.cos(1.571*moveInput) - cosMultiplier; } else { - steerOutput = 0; + moveOutput = 0; } - + + SmartDashboard.putNumber("Steer Output Test", moveOutput); m_drive.driveWithInput(moveOutput, steerOutput); }