Skip to content
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.

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

Driving to a position #58

Merged
merged 10 commits into from
Jan 25, 2025
Prev Previous commit
Next Next commit
got the code to make the robot drive to the closest position, will de…
…finetely require some cleaning up later.
DylanTaylor29 committed Jan 25, 2025
commit 34babb6af2242b5c5f8b56e17f19a3dc452ef057
15 changes: 14 additions & 1 deletion src/main/java/frc/robot/Robot.java
Original file line number Diff line number Diff line change
@@ -11,6 +11,7 @@
import edu.wpi.first.wpilibj.PowerDistribution.ModuleType;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
@@ -179,7 +180,7 @@ private void configureDriverButtonBindings() {
.ignoringDisable(true));

m_driver.AIn()
.whileTrue(new DriveToPoseCommand(m_swerve, m_vision, getNearestPose(m_swerve.getPose(), targetPoses)));
.whileTrue(createDriveToClosestPoseCommand());

}
// spotless:on
@@ -212,6 +213,17 @@ public double getPDHCurrent(int CANBusPort) {
new Pose2d(1.0, 5.0, Rotation2d.fromDegrees(0.0))
};

private Command createDriveToClosestPoseCommand() {
return new Command() {
@Override
public void initialize() {
Pose2d currentPose = m_swerve.getPose();
Pose2d closestPose = getNearestPose(currentPose, targetPoses);
new DriveToPoseCommand(m_swerve, m_vision, closestPose).schedule();
}
};
}

public Pose2d getNearestPose(Pose2d currentPose, Pose2d[] targetPoses) {
Pose2d closestPose = new Pose2d(5, 5, Rotation2d.fromDegrees(0));
double minDistance = Double.MAX_VALUE;
@@ -220,6 +232,7 @@ public Pose2d getNearestPose(Pose2d currentPose, Pose2d[] targetPoses) {
double distance = currentPose.getTranslation().getDistance(targetPose.getTranslation());

if (distance < minDistance) {
minDistance = distance;
closestPose = targetPose;
}
}