Skip to content

Commit

Permalink
Merge pull request #98 from Technologyman00/main
Browse files Browse the repository at this point in the history
Prevent Movement after Auto
  • Loading branch information
thenetworkgrinch authored Nov 22, 2023

Unverified

This commit is not signed, but one or more authors requires that any commit attributed to them is signed.
2 parents ae18892 + f3e65d7 commit 12c9744
Showing 1 changed file with 17 additions and 1 deletion.
Original file line number Diff line number Diff line change
@@ -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 initRotation = false;

/**
* Used to drive a swerve robot in full field-centric mode. vX and vY supply translation inputs, where x is
@@ -60,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.
@@ -68,11 +70,25 @@ 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(initRotation)
{
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));
}
//Dont Init Rotation Again
initRotation = false;
}

// Limit velocity to prevent tippy
Translation2d translation = SwerveController.getTranslation2d(desiredSpeeds);
translation = SwerveMath.limitVelocity(translation, swerve.getFieldVelocity(), swerve.getPose(),

0 comments on commit 12c9744

Please sign in to comment.