diff --git a/src/main/java/frc4388/robot/commands/LimeAlign.java b/src/main/java/frc4388/robot/commands/LimeAlign.java deleted file mode 100644 index c811a3e..0000000 --- a/src/main/java/frc4388/robot/commands/LimeAlign.java +++ /dev/null @@ -1,34 +0,0 @@ -// 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.wpilibj2.command.CommandBase; -import frc4388.robot.subsystems.SwerveDrive; - -public class LimeAlign extends CommandBase { - public LimeAlign(SwerveDrive drive) { - addRequirements(drive); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() {} - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) {} - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 2edb77d..d4a27fa 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -67,7 +67,9 @@ public class SwerveDrive extends SubsystemBase { } // Use the left joystick to set speed. Apply a cubic curve and the set max speed. - Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust); + Translation2d speed = leftStick.times(speedAdjust * leftStick.getNorm()); + // Translation2d speed = leftStick.times(speedAdjust / leftStick.getNorm()); + Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00), Math.pow(speed.getY(), 3.00)); // Convert field-relative speeds to robot-relative speeds. @@ -117,12 +119,17 @@ public class SwerveDrive extends SubsystemBase { } public void toggleGear(double angle) { - if (this.speedAdjust == SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW - && Math.abs(angle) < 2) { + if (this.speedAdjust == SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW && Math.abs(angle) < 2) { + this.speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST; + SwerveDriveConstants.ROTATION_SPEED = SwerveDriveConstants.MAX_ROT_SPEED; + SwerveDriveConstants.ROT_CORRECTION_SPEED = SwerveDriveConstants.CORRECTION_MAX; } else { + this.speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; + SwerveDriveConstants.ROTATION_SPEED = SwerveDriveConstants.MIN_ROT_SPEED; + SwerveDriveConstants.ROT_CORRECTION_SPEED = SwerveDriveConstants.CORRECTION_MIN; } }