Skip to content

fixed and added swerve methods #79

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
escapee-86 wants to merge 2 commits into
base: main
Choose a base branch
from
Open

fixed and added swerve methods #79

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
c69a532
fixed and added swerve methods
escapee-86 Mar 28, 2026
8d2b366
IM LOSING MY MIND
escapee-86 Mar 31, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
14 changes: 13 additions & 1 deletion src/main/java/frc/robot/Robot.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,8 @@
import static frc.robot.Constants.RobotK.*;
import static frc.robot.Constants.ShooterK;
import java.util.Optional;
import java.util.function.Supplier;

import org.photonvision.EstimatedRobotPose;
import org.photonvision.PhotonCamera;

Expand All @@ -19,6 +21,9 @@

import choreo.auto.AutoFactory;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Transform2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.units.measure.AngularVelocity;
import edu.wpi.first.units.measure.LinearVelocity;
import edu.wpi.first.wpilibj.DataLogManager;
Expand Down Expand Up @@ -154,6 +159,8 @@ public class Robot extends TimedRobot {
// private final Tracer m_periodicTracer = new Tracer();
private final Command m_preheaterCommand;

private Supplier<Pose2d> supp_robotPose = () -> m_drivetrain.getState().Pose;

/* CONSTRUCTOR */
public Robot() {
configureBindings();
Expand Down Expand Up @@ -286,6 +293,11 @@ private void configureBindings() {
m_manipulator.povUp().onTrue(m_intake.setIntakeFlapServoCmd(IntakeK.kIntakeFlapDeployPos));
m_manipulator.povDown().onTrue(m_intake.setIntakeFlapServoCmd(0));

m_driver.povUp().whileTrue(m_drivetrain.translateRobot(1, 0, 0.1));
m_driver.povLeft().whileTrue(m_drivetrain.translateRobot(0, 1, 0.1));
m_driver.povDown().whileTrue(m_drivetrain.translateRobot(-1, 0, 0.1));
m_driver.povRight().whileTrue(m_drivetrain.translateRobot(0, -1, 0.1));

// snapshot on each shoot press
trg_shoot.onTrue(WaltCamera.takeSnapshotCmd());

Expand Down Expand Up @@ -372,7 +384,7 @@ public void robotPeriodic() {
CommandScheduler.getInstance().run();
// m_periodicTracer.addEpoch("CommandScheduler");

log_robotPose.accept(m_drivetrain.getState().Pose);
log_robotPose.accept(supp_robotPose.get());

for (var camera : WaltCamera.AllCameras) {
Optional<EstimatedRobotPose> estimatedPoseOptional = camera.getEstimatedGlobalPose();
Expand Down
136 changes: 123 additions & 13 deletions src/main/java/frc/robot/subsystems/Swerve.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,7 @@
import static edu.wpi.first.units.Units.*;

import java.util.Optional;
import java.util.Set;
import java.util.function.Supplier;

import org.photonvision.targeting.PhotonTrackedTarget;
Expand All @@ -24,6 +25,8 @@
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Transform2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
import edu.wpi.first.math.numbers.N1;
Expand Down Expand Up @@ -400,22 +403,129 @@ public AutoFactory createAutoFactory(TrajectoryLogger<SwerveSample> trajLogger)
}

/**
* robot goes to specified pose
* @param destination
* @return
* @param desPose Posd2d to move to
* @param tolerance how close to pose until stopping
* @return a Command that makes the robot move to the desired Pose2d
*/
public Command toPose(Pose2d destination) {
return Commands.run(
() -> {
Pose2d curPose = getState().Pose;
public Command roboToPose(Pose2d desPose, double tolerance) {
return Commands.defer(() -> {
Pose2d initPose = getState().Pose;
return Commands.runOnce(() -> {

@Bankst Bankst Apr 1, 2026

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

this is absolutely not going to work. the previous code using Commands.run() is correct. With runOnce, you only calculate once so you never update the controllers after the robot moves

double xSpeed = m_pathXController.calculate(initPose.getX(), desPose.getX());
double ySpeed = m_pathYController.calculate(initPose.getY(), desPose.getY());
double thetaSpeed = m_pathThetaController.calculate(initPose.getRotation().getRadians(), desPose.getRotation().getRadians());
setControl(swreq_drive.withVelocityX(xSpeed).withVelocityY(ySpeed).withRotationalRate(thetaSpeed));
}).andThen(Commands.waitUntil(() -> isNearPose(getState().Pose, desPose, tolerance)))
.andThen(() -> setControl(swreq_drive.withVelocityX(0).withVelocityY(0).withRotationalRate(0)));
}, Set.of(this));
}

double xSpeed = m_pathXController.calculate(curPose.getX(), destination.getX());
double ySpeed = m_pathYController.calculate(curPose.getY(), destination.getY());
double thetaSpeed = m_pathThetaController.calculate(curPose.getRotation().getRadians(), destination.getRotation().getRadians());
public boolean isNearPose(Pose2d curPose, Pose2d desPose, double translationTolerance, double rotationTolerance) {
return isNearTranslation(curPose.getTranslation(), desPose.getTranslation(), translationTolerance)
&& isNearRotation(curPose.getRotation(), desPose.getRotation(), rotationTolerance);
}

public boolean isNearPose(Pose2d curPose, Pose2d desPose, double translationTolerance) {
return isNearTranslation(curPose.getTranslation(), desPose.getTranslation(), translationTolerance);
}

/**
* @param xTranslation meters in the x direction to translate by
* @param yTranslation meters in the y direction to translate by
* @param translationTolerance how close to desired translations until stopping
* @param rotationRads radians to rotate robot
* @param rotationTolerance how close to desired turn until stopping
* @return a Command that makes the robot go to the desired Translation2d and rotates the robot the specified number of radians
*/
public Command translateAndRotateRobot(double xTranslation, double yTranslation, double translationTolerance, double rotationRads, double rotationTolerance) {
return Commands.defer(() -> {
Pose2d initPose = getState().Pose;
return Commands.runOnce(() -> {
double xSpeed = m_pathXController.calculate(initPose.getX(), initPose.getX() + xTranslation) / 2;
double ySpeed = m_pathYController.calculate(initPose.getY(), initPose.getY() + yTranslation) / 2;
double thetaSpeed = m_pathThetaController.calculate(initPose.getRotation().getRadians(), initPose.getRotation().getRadians() + rotationRads) / 2;
setControl(swreq_drive.withVelocityX(xSpeed).withVelocityY(ySpeed).withRotationalRate(thetaSpeed));
}
);
}).andThen(Commands.waitUntil(() -> isNearPose(getState().Pose, initPose.plus(new Transform2d(xTranslation, yTranslation, new Rotation2d(Radians.of(rotationRads)))), translationTolerance, rotationTolerance)))
.andThen(() -> setControl(swreq_drive.withVelocityX(0).withVelocityY(0).withRotationalRate(0)));
}, Set.of(this));
}

/**
* @param desTranslation Translation2d to go to
* @param tolerance how close to translation until stopping
* @return a Command that makes the robot go to the desired Translation2d
*/
public Command roboToTranslation(Translation2d desTranslation, double tolerance) {
return Commands.defer(() -> {
Translation2d initTranslation = getState().Pose.getTranslation();
return Commands.runOnce(() -> {
double xSpeed = m_pathXController.calculate(initTranslation.getX(), desTranslation.getX()) / 2;
double ySpeed = m_pathYController.calculate(initTranslation.getY(), desTranslation.getY()) / 2;
setControl(swreq_drive.withVelocityX(xSpeed).withVelocityY(ySpeed));
}).andThen(Commands.waitUntil(() -> isNearTranslation(getState().Pose.getTranslation(), desTranslation, tolerance)))
.andThen(() -> setControl(swreq_drive.withVelocityX(0).withVelocityY(0).withRotationalRate(0)));
}, Set.of(this));
}

public boolean isNearTranslation(Translation2d curTranslation, Translation2d desTranslation, double tolerance) {
return Math.hypot(
desTranslation.getMeasureX().minus(curTranslation.getMeasureX()).baseUnitMagnitude(),
desTranslation.getMeasureY().minus(curTranslation.getMeasureY()).baseUnitMagnitude()
) <= tolerance;
}

/**
* @param xTranslation meters in the x direction to translate by
* @param yTranslation meters in the y direction to translate by
* @param tolerance how close to desired translations until stopping
* @return a Command that translates the robot by the specified x and y distances
*/
public Command translateRobot(double xTranslation, double yTranslation, double tolerance) {
return Commands.defer(() -> {
Translation2d initTranslation = getState().Pose.getTranslation();
return Commands.runOnce(() -> {
double xSpeed = m_pathXController.calculate(initTranslation.getX(), initTranslation.getX() + xTranslation) / 2;
double ySpeed = m_pathYController.calculate(initTranslation.getY(), initTranslation.getY() + yTranslation) / 2;
setControl(swreq_drive.withVelocityX(xSpeed).withVelocityY(ySpeed));
}).andThen(Commands.waitUntil(() -> isNearTranslation(getState().Pose.getTranslation(), initTranslation.plus(new Translation2d(xTranslation, yTranslation)), tolerance)))
.andThen(() -> setControl(swreq_drive.withVelocityX(0).withVelocityY(0).withRotationalRate(0)));
}, Set.of(this));
}

/**
* @param desRotation Rotation2d to turn to
* @param tolerance how close to rotation until stopping
* @return a Command that makes the robot turn to the desired Rotation2d
*/
public Command roboToRotation(Rotation2d desRotation, double tolerance) {
return Commands.defer(() -> {
Rotation2d initRotation = getState().Pose.getRotation();
return Commands.runOnce(() -> {
double thetaSpeed = m_pathThetaController.calculate(initRotation.getRadians(), desRotation.getRadians()) / 2;
setControl(swreq_drive.withRotationalRate(thetaSpeed));
}).andThen(Commands.waitUntil(() -> isNearRotation(getState().Pose.getRotation(), desRotation, tolerance)))
.andThen(() -> setControl(swreq_drive.withRotationalRate(0)));
}, Set.of(this));
}

public boolean isNearRotation(Rotation2d curRotation, Rotation2d desRotation, double tolerance) {
return Radians.of(curRotation.getRadians()).isNear(Radians.of(desRotation.getRadians()), Radians.of(tolerance));
}

/**
* @param rotationRads radians to rotate robot
* @param tolerance how close to desired turn until stopping
* @return a Command that rotates the robot the specified number of radians
*/
public Command rotateRobot(double rotationRads, double tolerance) {
return Commands.defer(() -> {
Rotation2d initRotation = getState().Pose.getRotation();
return Commands.runOnce(() -> {
double thetaSpeed = m_pathThetaController.calculate(initRotation.getRadians(), initRotation.getRadians() + rotationRads) / 2;
setControl(swreq_drive.withRotationalRate(thetaSpeed));
}).andThen(Commands.waitUntil(() -> isNearRotation(getState().Pose.getRotation(), initRotation.plus(new Rotation2d(Radians.of(rotationRads))), tolerance)))
.andThen(() -> setControl(swreq_drive.withRotationalRate(0)));
}, Set.of(this));
}

/**
Expand All @@ -426,7 +536,7 @@ public Command swerveToObject() {
Pose2d destination = detection.targetToPose(getState().Pose, target);
detection.addFuel(destination);

return toPose(destination);
return roboToPose(destination, 0.1);
}

public static Pose2d faceFuelPose(Pose2d robotPose, Pose2d fuelLocation) {
Expand Down