From c4623f9606ca186c7f93d6e4bc3d42ba198d4493 Mon Sep 17 00:00:00 2001 From: Technologyman00 Date: Mon, 20 Nov 2023 20:46:20 -0600 Subject: [PATCH 1/5] Prevent Movement after Auto --- .../swervedrive/drivebase/AbsoluteDrive.java | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDrive.java b/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDrive.java index b76cfefdb..bf7d7e72f 100644 --- a/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDrive.java +++ b/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDrive.java @@ -60,6 +60,23 @@ public AbsoluteDrive(SwerveSubsystem swerve, DoubleSupplier vX, DoubleSupplier v @Override public void initialize() { + // Prevent Movement After Auto + // Get the curretHeading + double currentHeading = swerve.getHeading().getRadians(); + + // Set the Current Heading to the desired Heading + ChassisSpeeds desiredSpeeds = swerve.getTargetSpeeds(0, 0, Math.sin(currentHeading), Math.cos(currentHeading)); + + //Calculate Translation to send to drive + Translation2d translation = SwerveController.getTranslation2d(desiredSpeeds); + translation = SwerveMath.limitVelocity(translation, swerve.getFieldVelocity(), swerve.getPose(), + Constants.LOOP_TIME, Constants.ROBOT_MASS, List.of(Constants.CHASSIS), + swerve.getSwerveDriveConfiguration()); + SmartDashboard.putNumber("LimitedTranslation", translation.getX()); + SmartDashboard.putString("Translation", translation.toString()); + + // Make the robot not move from its position after auto + swerve.drive(translation, desiredSpeeds.omegaRadiansPerSecond, true); } // Called every time the scheduler runs while the command is scheduled. From da009983c909321d38527e83443cd3f7647b5e00 Mon Sep 17 00:00:00 2001 From: Technologyman00 Date: Tue, 21 Nov 2023 17:59:05 -0600 Subject: [PATCH 2/5] Changed to not repeat code --- .../swervedrive/drivebase/AbsoluteDrive.java | 31 ++++++++----------- 1 file changed, 13 insertions(+), 18 deletions(-) diff --git a/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDrive.java b/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDrive.java index bf7d7e72f..bd38ec914 100644 --- a/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDrive.java +++ b/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDrive.java @@ -24,6 +24,7 @@ public class AbsoluteDrive extends CommandBase private final SwerveSubsystem swerve; private final DoubleSupplier vX, vY; private final DoubleSupplier headingHorizontal, headingVertical; + private boolean firstLoop = true; /** * Used to drive a swerve robot in full field-centric mode. vX and vY supply translation inputs, where x is @@ -60,23 +61,6 @@ public AbsoluteDrive(SwerveSubsystem swerve, DoubleSupplier vX, DoubleSupplier v @Override public void initialize() { - // Prevent Movement After Auto - // Get the curretHeading - double currentHeading = swerve.getHeading().getRadians(); - - // Set the Current Heading to the desired Heading - ChassisSpeeds desiredSpeeds = swerve.getTargetSpeeds(0, 0, Math.sin(currentHeading), Math.cos(currentHeading)); - - //Calculate Translation to send to drive - Translation2d translation = SwerveController.getTranslation2d(desiredSpeeds); - translation = SwerveMath.limitVelocity(translation, swerve.getFieldVelocity(), swerve.getPose(), - Constants.LOOP_TIME, Constants.ROBOT_MASS, List.of(Constants.CHASSIS), - swerve.getSwerveDriveConfiguration()); - SmartDashboard.putNumber("LimitedTranslation", translation.getX()); - SmartDashboard.putString("Translation", translation.toString()); - - // Make the robot not move from its position after auto - swerve.drive(translation, desiredSpeeds.omegaRadiansPerSecond, true); } // Called every time the scheduler runs while the command is scheduled. @@ -85,11 +69,22 @@ public void execute() { // Get the desired chassis speeds based on a 2 joystick module. - ChassisSpeeds desiredSpeeds = swerve.getTargetSpeeds(vX.getAsDouble(), vY.getAsDouble(), headingHorizontal.getAsDouble(), headingVertical.getAsDouble()); + // Prevent Movement After Auto + if(firstLoop){ + // Get the curretHeading + double firstLoopHeading = swerve.getHeading().getRadians(); + + // Set the Current Heading to the desired Heading + desiredSpeeds = swerve.getTargetSpeeds(0, 0, Math.sin(firstLoopHeading), Math.cos(firstLoopHeading)); + + //No Longer First Loop + firstLoop = false; + } + // Limit velocity to prevent tippy Translation2d translation = SwerveController.getTranslation2d(desiredSpeeds); translation = SwerveMath.limitVelocity(translation, swerve.getFieldVelocity(), swerve.getPose(), From f10bb5864ab50fe02e07dd43343ae940be6b20c5 Mon Sep 17 00:00:00 2001 From: Technologyman00 Date: Tue, 21 Nov 2023 18:05:06 -0600 Subject: [PATCH 3/5] Adjusted not moving after auto to allow movement on first loop --- .../frc/robot/commands/swervedrive/drivebase/AbsoluteDrive.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDrive.java b/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDrive.java index bd38ec914..6d3e3f2b1 100644 --- a/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDrive.java +++ b/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDrive.java @@ -74,7 +74,7 @@ public void execute() headingVertical.getAsDouble()); // Prevent Movement After Auto - if(firstLoop){ + if(firstLoop && headingHorizontal.getAsDouble() != 0 && headingVertical.getAsDouble() != 0){ // Get the curretHeading double firstLoopHeading = swerve.getHeading().getRadians(); From 68943e680841ff39c0aed9b86153b9219c9348c1 Mon Sep 17 00:00:00 2001 From: Technologyman00 Date: Tue, 21 Nov 2023 18:09:07 -0600 Subject: [PATCH 4/5] Fixed Wrong Logic of First Loop and Movement --- .../swervedrive/drivebase/AbsoluteDrive.java | 19 +++++++++++-------- 1 file changed, 11 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDrive.java b/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDrive.java index 6d3e3f2b1..75e353509 100644 --- a/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDrive.java +++ b/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDrive.java @@ -73,14 +73,17 @@ public void execute() headingHorizontal.getAsDouble(), headingVertical.getAsDouble()); - // Prevent Movement After Auto - if(firstLoop && headingHorizontal.getAsDouble() != 0 && headingVertical.getAsDouble() != 0){ - // Get the curretHeading - double firstLoopHeading = swerve.getHeading().getRadians(); - - // Set the Current Heading to the desired Heading - desiredSpeeds = swerve.getTargetSpeeds(0, 0, Math.sin(firstLoopHeading), Math.cos(firstLoopHeading)); - + // Prevent Movement After Auto + if(firstLoop) + { + if(headingHorizontal.getAsDouble() == 0 && headingVertical.getAsDouble() == 0) + { + // Get the curretHeading + double firstLoopHeading = swerve.getHeading().getRadians(); + + // Set the Current Heading to the desired Heading + desiredSpeeds = swerve.getTargetSpeeds(0, 0, Math.sin(firstLoopHeading), Math.cos(firstLoopHeading)); + } //No Longer First Loop firstLoop = false; } From f3e65d70e0fb68ca13b8974eed97299c15273a72 Mon Sep 17 00:00:00 2001 From: Technologyman00 Date: Tue, 21 Nov 2023 19:07:22 -0600 Subject: [PATCH 5/5] Added setting init to true in initilalize() --- .../commands/swervedrive/drivebase/AbsoluteDrive.java | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDrive.java b/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDrive.java index 75e353509..00dfbd282 100644 --- a/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDrive.java +++ b/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDrive.java @@ -24,7 +24,7 @@ public class AbsoluteDrive extends CommandBase private final SwerveSubsystem swerve; private final DoubleSupplier vX, vY; private final DoubleSupplier headingHorizontal, headingVertical; - private boolean firstLoop = true; + private boolean initRotation = false; /** * Used to drive a swerve robot in full field-centric mode. vX and vY supply translation inputs, where x is @@ -61,6 +61,7 @@ public AbsoluteDrive(SwerveSubsystem swerve, DoubleSupplier vX, DoubleSupplier v @Override public void initialize() { + initRotation = true; } // Called every time the scheduler runs while the command is scheduled. @@ -74,7 +75,7 @@ public void execute() headingVertical.getAsDouble()); // Prevent Movement After Auto - if(firstLoop) + if(initRotation) { if(headingHorizontal.getAsDouble() == 0 && headingVertical.getAsDouble() == 0) { @@ -84,8 +85,8 @@ public void execute() // Set the Current Heading to the desired Heading desiredSpeeds = swerve.getTargetSpeeds(0, 0, Math.sin(firstLoopHeading), Math.cos(firstLoopHeading)); } - //No Longer First Loop - firstLoop = false; + //Dont Init Rotation Again + initRotation = false; } // Limit velocity to prevent tippy