generated from OakvilleDynamics/frc-robot-template
-
Notifications
You must be signed in to change notification settings - Fork 1
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Added part of our fork of YAGSL Example, which is not updated to 2024. This will fail builds and deploy.
- Loading branch information
1 parent
c5333b6
commit 9733d40
Showing
8 changed files
with
818 additions
and
32 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
|
|
@@ -12,6 +12,7 @@ | |
| * call. | ||
| */ | ||
| public final class Main { | ||
|
|
||
| private Main() {} | ||
|
|
||
| /** | ||
|
|
||
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
135 changes: 135 additions & 0 deletions
135
src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDrive.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,135 @@ | ||
| // 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 frc.robot.commands.swervedrive.drivebase; | ||
|
|
||
| import edu.wpi.first.math.geometry.Rotation2d; | ||
| import edu.wpi.first.math.geometry.Translation2d; | ||
| import edu.wpi.first.math.kinematics.ChassisSpeeds; | ||
| import edu.wpi.first.util.sendable.SendableBuilder; | ||
| import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; | ||
| import edu.wpi.first.wpilibj2.command.CommandBase; | ||
| import frc.robot.Constants; | ||
| import frc.robot.subsystems.swervedrive.SwerveSubsystem; | ||
| import java.util.List; | ||
| import java.util.function.DoubleSupplier; | ||
| import swervelib.SwerveController; | ||
| import swervelib.math.SwerveMath; | ||
|
|
||
| /** An example command that uses an example subsystem. */ | ||
| 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 torwards/away from alliance wall and y is left/right. headingHorzontal and | ||
| * headingVertical are the Cartesian coordinates from which the robot's angle will be derived— | ||
| * they will be converted to a polar angle, which the robot will rotate to. | ||
| * | ||
| * @param swerve The swerve drivebase subsystem. | ||
| * @param vX DoubleSupplier that supplies the x-translation joystick input. Should be in the range | ||
| * -1 to 1 with deadband already accounted for. Positive X is away from the alliance wall. | ||
| * @param vY DoubleSupplier that supplies the y-translation joystick input. Should be in the range | ||
| * -1 to 1 with deadband already accounted for. Positive Y is towards the left wall when | ||
| * looking through the driver station glass. | ||
| * @param headingHorizontal DoubleSupplier that supplies the horizontal component of the robot's | ||
| * heading angle. In the robot coordinate system, this is along the same axis as vY. Should | ||
| * range from -1 to 1 with no deadband. Positive is towards the left wall when looking through | ||
| * the driver station glass. | ||
| * @param headingVertical DoubleSupplier that supplies the vertical component of the robot's | ||
| * heading angle. In the robot coordinate system, this is along the same axis as vX. Should | ||
| * range from -1 to 1 with no deadband. Positive is away from the alliance wall. | ||
| */ | ||
| public AbsoluteDrive( | ||
| SwerveSubsystem swerve, | ||
| DoubleSupplier vX, | ||
| DoubleSupplier vY, | ||
| DoubleSupplier headingHorizontal, | ||
| DoubleSupplier headingVertical) { | ||
| this.swerve = swerve; | ||
| this.vX = vX; | ||
| this.vY = vY; | ||
| this.headingHorizontal = headingHorizontal; | ||
| this.headingVertical = headingVertical; | ||
|
|
||
| addRequirements(swerve); | ||
| } | ||
|
|
||
| @Override | ||
| public void initialize() { | ||
| initRotation = true; | ||
| } | ||
|
|
||
| // Called every time the scheduler runs while the command is scheduled. | ||
| @Override | ||
| 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 | ||
| Rotation2d firstLoopHeading = swerve.getHeading(); | ||
|
|
||
| // Set the Current Heading to the desired Heading | ||
| desiredSpeeds = | ||
| swerve.getTargetSpeeds(0, 0, firstLoopHeading.getSin(), firstLoopHeading.getCos()); | ||
| } | ||
| // Dont Init Rotation Again | ||
| initRotation = false; | ||
| } | ||
|
|
||
| // Limit velocity to prevent tippy | ||
| 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 move | ||
| swerve.drive(translation, desiredSpeeds.omegaRadiansPerSecond, true); | ||
| } | ||
|
|
||
| // 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; | ||
| } | ||
|
|
||
| @Override | ||
| public void initSendable(SendableBuilder builder) { | ||
| builder.addDoubleProperty( | ||
| "Target Rotation Speed", | ||
| () -> | ||
| swerve.getTargetSpeeds( | ||
| vX.getAsDouble(), | ||
| vY.getAsDouble(), | ||
| headingHorizontal.getAsDouble(), | ||
| headingVertical.getAsDouble()) | ||
| .omegaRadiansPerSecond, | ||
| null); | ||
| } | ||
| } |
Oops, something went wrong.