diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 21510fc..401f516 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -172,8 +172,8 @@ public void initDefaultCommands_teleop() { () -> { if (swerveDrive.getTurnToAngleMode()) { return ( - driverController.getRightX() > 0.0 - || driverController.getRightY() > 0.0 + Math.abs(driverController.getRightX()) > 0.05 + || Math.abs(driverController.getRightY()) > 0.05 ); } else { @@ -182,23 +182,16 @@ public void initDefaultCommands_teleop() { }, // () -> false, // Turn to angle (disabled) () -> { // Turn To angle Direction - if (Math.abs(driverController.getRightY()) > Math.abs(driverController.getRightX())) { - if (driverController.getRightY() < 0) { - return 0.0; + double xValue = commandDriverController.getRightX(); + double yValue = commandDriverController.getRightY(); + double magnitude = Math.sqrt((xValue*xValue) + (yValue*yValue)); + if (magnitude > 0.49) { + double angle = (90 + NerdyMath.radiansToDegrees(Math.atan2(commandDriverController.getRightY(), commandDriverController.getRightX()))); + angle = (((-1 * angle) % 360) + 360) % 360; + SmartDashboard.putNumber("desired angle", angle); + return angle; } - else if (driverController.getRightY() > 0) { - return 180.0; - } - } - else if ((Math.abs(driverController.getRightY()) < Math.abs(driverController.getRightX()))) { - if (driverController.getRightX() < 0) { - return 90.0; - } - else if (driverController.getRightX() > 0) { - return 270.0; - } - } - return 0.0; + return 1000.0; }); swerveDrive.setDefaultCommand(swerveJoystickCommand); diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/swerve/SwerveDrivetrain.java index 39abe0f..95fe87a 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveDrivetrain.java @@ -480,7 +480,7 @@ public Command turnToSubwoofer(double angleTolerance) { return command; } - public boolean turnToAngleMode = false; + public boolean turnToAngleMode = true; public Command toggleTurnToAngleMode() { return Commands.runOnce(() -> turnToAngleMode = !turnToAngleMode);