Skip to content

Commit

Permalink
Flip driver control on the red alliance (#30)
Browse files Browse the repository at this point in the history
* fix: use correct version of LocalADStarAK

* feat: flip driver control on red alliance

* fix: check flipping in intialize

---------

Co-authored-by: Zach Rutman <[email protected]>
  • Loading branch information
mimizh2418 and rutmanz authored Feb 9, 2024
1 parent 7868c9d commit dfdb03f
Show file tree
Hide file tree
Showing 2 changed files with 24 additions and 15 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,7 @@
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.wpilibj.DriverStation;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import org.team1540.robot2024.subsystems.drive.Drivetrain;
Expand All @@ -19,6 +20,8 @@ public class SwerveDriveCommand extends Command {
private final SlewRateLimiter yLimiter = new SlewRateLimiter(2);
private final SlewRateLimiter rotLimiter = new SlewRateLimiter(3);

private boolean isFlipped;

public SwerveDriveCommand(Drivetrain drivetrain, CommandXboxController controller) {
this.drivetrain = drivetrain;
this.controller = controller;
Expand All @@ -30,6 +33,9 @@ public void initialize() {
xLimiter.reset(0);
yLimiter.reset(0);
rotLimiter.reset(0);
isFlipped =
DriverStation.getAlliance().isPresent()
&& DriverStation.getAlliance().get() == DriverStation.Alliance.Red;
}

@Override
Expand All @@ -54,7 +60,9 @@ public void execute() {
linearVelocity.getX() * drivetrain.getMaxLinearSpeedMetersPerSec(),
linearVelocity.getY() * drivetrain.getMaxLinearSpeedMetersPerSec(),
omega * drivetrain.getMaxAngularSpeedRadPerSec(),
drivetrain.getRotation()
isFlipped
? drivetrain.getRotation().plus(Rotation2d.fromDegrees(180))
: drivetrain.getRotation()
)
);
}
Expand Down
29 changes: 15 additions & 14 deletions src/main/java/org/team1540/robot2024/util/LocalADStarAK.java
Original file line number Diff line number Diff line change
Expand Up @@ -29,7 +29,7 @@ public class LocalADStarAK implements Pathfinder {
*/
@Override
public boolean isNewPathAvailable() {
if (Logger.hasReplaySource()) {
if (!Logger.hasReplaySource()) {
io.updateIsNewPathAvailable();
}

Expand All @@ -41,13 +41,13 @@ public boolean isNewPathAvailable() {
/**
* Get the most recently calculated path
*
* @param constraints The path constraints to use when creating the path
* @param constraints The path constraints to use when creating the path
* @param goalEndState The goal end state to use when creating the path
* @return The PathPlannerPath created from the points calculated by the pathfinder
*/
@Override
public PathPlannerPath getCurrentPath(PathConstraints constraints, GoalEndState goalEndState) {
if (Logger.hasReplaySource()) {
if (!Logger.hasReplaySource()) {
io.updateCurrentPathPoints(constraints, goalEndState);
}

Expand All @@ -64,11 +64,11 @@ public PathPlannerPath getCurrentPath(PathConstraints constraints, GoalEndState
* Set the start position to pathfind from
*
* @param startPosition Start position on the field. If this is within an obstacle it will be
* moved to the nearest non-obstacle node.
* moved to the nearest non-obstacle node.
*/
@Override
public void setStartPosition(Translation2d startPosition) {
if (Logger.hasReplaySource()) {
if (!Logger.hasReplaySource()) {
io.adStar.setStartPosition(startPosition);
}
}
Expand All @@ -77,27 +77,29 @@ public void setStartPosition(Translation2d startPosition) {
* Set the goal position to pathfind to
*
* @param goalPosition Goal position on the field. f this is within an obstacle it will be moved
* to the nearest non-obstacle node.
* to the nearest non-obstacle node.
*/
@Override
public void setGoalPosition(Translation2d goalPosition) {
if (Logger.hasReplaySource()) {
if (!Logger.hasReplaySource()) {
io.adStar.setGoalPosition(goalPosition);
}
}

/**
* Set the dynamic obstacles that should be avoided while pathfinding.
*
* @param obs A List of Translation2d pairs representing obstacles. Each Translation2d represents
* opposite corners of a bounding box.
* @param obs A List of Translation2d pairs representing obstacles. Each Translation2d represents
* opposite corners of a bounding box.
* @param currentRobotPos The current position of the robot. This is needed to change the start
* position of the path to properly avoid obstacles
* position of the path to properly avoid obstacles
*/
@Override
public void setDynamicObstacles(
List<Pair<Translation2d, Translation2d>> obs, Translation2d currentRobotPos) {
io.adStar.setDynamicObstacles(obs, currentRobotPos);
if (!Logger.hasReplaySource()) {
io.adStar.setDynamicObstacles(obs, currentRobotPos);
}
}

private static class ADStarIO implements LoggableInputs {
Expand Down Expand Up @@ -128,8 +130,7 @@ public void fromLog(LogTable table) {

List<PathPoint> pathPoints = new ArrayList<>();
for (int i = 0; i < pointsLogged.length; i += 2) {
pathPoints.add(
new PathPoint(new Translation2d(pointsLogged[i], pointsLogged[i + 1]), null));
pathPoints.add(new PathPoint(new Translation2d(pointsLogged[i], pointsLogged[i + 1]), null));
}

currentPathPoints = pathPoints;
Expand All @@ -149,4 +150,4 @@ public void updateCurrentPathPoints(PathConstraints constraints, GoalEndState go
}
}
}
}
}

0 comments on commit dfdb03f

Please sign in to comment.