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

Flip driver control on the red alliance #30

Merged
merged 4 commits into from
Feb 9, 2024
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
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
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
}
}
}
}
}
Loading