Skip to content

Added the Driving Competition Mode and Values #42

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
Carbonic-Acidd wants to merge 1 commit into
base: main
Choose a base branch
from
Open

Added the Driving Competition Mode and Values #42

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
9 changes: 9 additions & 0 deletions src/main/java/frc/lib/controllers/VirtualXboxController.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -52,6 +52,15 @@ public Command rumbleFor(double duration, RumbleType rumbleType, double value) {
.finallyDo(() -> getHID().setRumble(RumbleType.kBothRumble, 0.0));
}

// New
public Command rumble(RumbleType rumbleType, double value) {
return Commands.run(() -> getHID().setRumble(rumbleType, value));
}

public Command stopRumble() {
return Commands.run(() -> getHID().setRumble(RumbleType.kBothRumble, 0.0));
}

@Override
public Trigger leftBumper(EventLoop loop) {
return super.leftBumper(loop)
Expand Down
2 changes: 2 additions & 0 deletions src/main/java/frc/robot/Constants.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -472,6 +472,8 @@ public static final class SwerveConstants {
public static final double maxAngularSpeed = Units.degreesToRadians(180);
public static final double slowMotionMaxTranslationalSpeed = Units.feetToMeters(3.53);
public static final double turboMaxTranslationalSpeed = Units.feetToMeters(14.5);
// New
public static final double bufferMaxTranslationalSpeed = Units.feetToMeters(1.0);

public static final double maxTranslationalAcceleration = Units.feetToMeters(18.0);
public static final double maxAngularAcceleration = Units.feetToMeters(270.0);
Expand Down
96 changes: 96 additions & 0 deletions src/main/java/frc/robot/RobotContainer.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -7,12 +7,14 @@
import com.pathplanner.lib.auto.AutoBuilder;
import com.pathplanner.lib.auto.NamedCommands;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.DataLogManager;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.GenericHID.RumbleType;
import edu.wpi.first.wpilibj.PowerDistribution;
import edu.wpi.first.wpilibj.RobotBase;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.util.Color;
Expand Down Expand Up @@ -89,6 +91,18 @@ public class RobotContainer implements Logged {
private Alert swervePrematchAlert = new Alert("", AlertType.INFO);
private Alert generalPrematchAlert = new Alert("", AlertType.INFO);

// New
Pose2d currentPose = swerve.getPose();
double xCoordinate = currentPose.getX();
double yCoordinate = currentPose.getY();
private final double stopLength = .3;
private final double MAX_X = swerve.MAX_X;
private final double MIN_X = swerve.MIN_X;
private final double MAX_Y = swerve.MAX_Y;
private final double MIN_Y = swerve.MIN_Y;
private double stopTime = 0;
private boolean isStopped = false;

/** The container for the robot. Contains subsystems, OI devices, and commands. */
public RobotContainer() {
DataLogManager.log("Initializing Robot Container");
Expand Down Expand Up @@ -291,6 +305,11 @@ public RobotContainer() {
new Trigger(intake::beamBroken)
.and(RobotModeTriggers.teleop())
.onTrue(driverController.rumbleFor(0.25, RumbleType.kRightRumble, 1.0));
// New
new Trigger(swerve::isNearPracticeOutOfBounds)
.and(RobotModeTriggers.teleop())
.onTrue(driverController.rumble(RumbleType.kBothRumble, .5))
.onFalse(driverController.stopRumble());

new Trigger(() -> DriverStation.getMatchTime() > 0.0 && DriverStation.getMatchTime() < 23.0)
.and(RobotModeTriggers.teleop())
Expand Down Expand Up @@ -349,6 +368,82 @@ private void configureDriveBindings() {

driverController.x().whileTrue(swerve.run(swerve::lockModules));

// New
driverController
.b()
.and(driverController.a())
.onTrue(
Commands.sequence(
Commands.runOnce(
() -> {
swerve.setZeroPosition();
SmartDashboard.putString("Status", "Indoor Drive Competition Mode Enabled");
}),
new TeleopSwerve(
driverController::getLeftY,
driverController::getLeftX,
driverController::getRightX,
driverController::getRightY,
driverController::getLeftBumper,
() -> {
double currentTime = Timer.getFPGATimestamp();

if (xCoordinate >= MAX_X - 2) {
return Math.max(
0.1, SwerveConstants.maxTranslationalSpeed * (MAX_X - xCoordinate) / 2);
} else if (xCoordinate <= MIN_X + 2) {
return Math.max(
0.1, SwerveConstants.maxTranslationalSpeed * (xCoordinate - MIN_X) / 2);
} else if (yCoordinate >= MAX_Y - 2) {
return Math.max(
0.1, SwerveConstants.maxTranslationalSpeed * (MAX_Y - yCoordinate) / 2);
} else if (yCoordinate <= MIN_Y + 2) {
return Math.max(
0.1, SwerveConstants.maxTranslationalSpeed * (yCoordinate - MIN_Y) / 2);
}

if (xCoordinate >= MAX_X - stopLength
|| xCoordinate <= MIN_X + stopLength
|| yCoordinate >= MAX_Y - stopLength
|| yCoordinate <= MIN_Y + stopLength) {

if (!isStopped) {
stopTime = currentTime;
isStopped = true;
}
return 0;
}

if (isStopped) {
if (currentTime - stopTime >= 2.0) {
isStopped = false;
} else {
return 0;
}
}

return SwerveConstants.maxTranslationalSpeed;
},
SwerveConstants.maxAngularSpeed,
swerve)));

// driverController.b().onTrue(
// new TeleopSwerve(driverController::getLeftY,
// driverController::getLeftX,
// driverController::getRightX,
// driverController::getRightY,
// driverController::getLeftBumper,
// () -> {
// if(xCoordinate > 7.5 || xCoordinate < -7.5 || yCoordinate > 5 || yCoordinate <-5) {
// driverController.rumbleFor(5, RumbleType.kBothRumble, 1.0);
// return SwerveConstants.bufferMaxTranslationalSpeed;
// }
// return SwerveConstants.maxTranslationalSpeed;
// },
// SwerveConstants.maxAngularSpeed
// , swerve)
// );

// slowMode
// .and(turnToSpeaker.negate())
// .whileTrue(
Expand All @@ -364,6 +459,7 @@ private void configureDriveBindings() {

driverController
.a()
.and(driverController.b().negate())
.onTrue(
Commands.runOnce(
() ->
Expand Down
30 changes: 30 additions & 0 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 @@ -87,6 +87,13 @@
import org.photonvision.targeting.PhotonTrackedTarget;

public class Swerve extends VirtualSubsystem implements Logged {
// New
private Pose2d zeroPosition = new Pose2d(0, 0, new Rotation2d());
public final double MIN_X = -7.5;
public final double MIN_Y = -5.0;
public final double MAX_X = 7.5;
public final double MAX_Y = 5.0;

public static record PoseEstimate(Pose3d estimatedPose, double timestamp, Vector<N3> standardDevs)
implements Comparable<PoseEstimate> {
@Override
Expand Down Expand Up @@ -618,6 +625,24 @@ public SwerveModuleState[] getDesiredStates() {
};
}

// New
Pose2d currentPose = getPose();
double xCoordinate = currentPose.getX();
double yCoordinate = currentPose.getY();

public boolean isNearPracticeOutOfBounds() {
if (xCoordinate >= MAX_X - 1) {
return true;
} else if (xCoordinate <= MIN_X + 1) {
return true;
} else if (yCoordinate >= MAX_Y - 1) {
return true;
} else if (yCoordinate <= MIN_Y + 1) {
return true;
}
return false;
}

private boolean odometryUpdateValid(Twist2d delta) {
Pose2d poseExponential = new Pose2d().exp(delta);

Expand Down Expand Up @@ -1002,6 +1027,11 @@ public void periodic() {
((double) (odometryUpdateCount - odometryRejectCount) / odometryUpdateCount) * 100.0);
}

// New
public void setZeroPosition() {
poseEstimator.resetPosition(getRawHeading(), getModulePositions(), zeroPosition);
}

@Override
public void simulationPeriodic() {
frontLeftModule.simulationPeriodic();
Expand Down