Team4388/2025RidgeScape
1
0
Fork 0
mirror of https://github.com/Team4388/2025RidgeScape.git synced 2026-06-08 16:28:04 -06:00
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #32 from Team4388/Create-TimesNegativeOne

Browse Source 
Unify way to times negative one
This commit is contained in:
C4llSqin
2025-02-26 17:00:17 -07:00
committed by GitHub
parent 15722e8a67 3cf162de74
commit 46674cc213
14 changed files with 247 additions and 77 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Robot.java
+2
View File
@@ -23,6 +23,7 @@ import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
import frc4388.utility.CanDevice;
import frc4388.utility.DeferredBlock;
import frc4388.utility.DeferredBlockMulti;
import frc4388.utility.RobotTime;
import frc4388.utility.Status;
import frc4388.utility.Subsystem;
@@ -113,6 +114,7 @@ public class Robot extends TimedRobot {
@Override
public void disabledExit() {
DeferredBlock.execute();
DeferredBlockMulti.execute();
}
/**
src/main/java/frc4388/robot/RobotContainer.java
+65 -30
View File
@@ -13,6 +13,7 @@ import edu.wpi.first.wpilibj.DriverStation;
import java.io.File;
import java.util.ArrayList;
import java.util.List;
import java.util.function.BooleanSupplier;
import edu.wpi.first.cameraserver.CameraServer;
import edu.wpi.first.math.geometry.Translation2d;
@@ -41,6 +42,7 @@ import edu.wpi.first.wpilibj2.command.Commands;
// Autos
import frc4388.utility.controller.VirtualController;
import frc4388.robot.commands.GotoLastApril;
import frc4388.robot.commands.IfCommand;
import frc4388.robot.commands.LidarAlign;
import frc4388.robot.commands.MoveForTimeCommand;
import frc4388.robot.commands.waitElevatorRefrence;
@@ -63,8 +65,10 @@ import frc4388.robot.subsystems.SwerveDrive;
// Utilites
import frc4388.utility.DeferredBlock;
import frc4388.utility.DeferredBlockMulti;
import frc4388.utility.ReefPositionHelper;
import frc4388.utility.Subsystem;
import frc4388.utility.TimesNegativeOne;
import frc4388.utility.ReefPositionHelper.Side;
import frc4388.utility.configurable.ConfigurableString;
@@ -117,11 +121,15 @@ public class RobotContainer {
// true, false);
// private Command AutoGotoPosition = new GotoLastApril(m_robotSwerveDrive, m_vision);
private Command AprilLidarAlignL4Right = new SequentialCommandGroup(
new InstantCommand(() -> {m_robotSwerveDrive.shiftDown();m_robotSwerveDrive.shiftDown();}),
new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
new IfCommand(() -> m_robotElevator.isL4Primed(), new SequentialCommandGroup(
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.RIGHT)
)),
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.RIGHT),
// new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
// new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.RIGHT),
new waitEndefectorRefrence(m_robotElevator),
new waitElevatorRefrence(m_robotElevator),
@@ -136,17 +144,22 @@ public class RobotContainer {
new MoveForTimeCommand(m_robotSwerveDrive,
new Translation2d(0,1), new Translation2d(), 500, true),
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator)
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator),
new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
);
private Command AprilLidarAlignL4Left = new SequentialCommandGroup(
new InstantCommand(() -> {m_robotSwerveDrive.shiftDown();m_robotSwerveDrive.shiftDown();}),
new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
new IfCommand(() -> m_robotElevator.isL4Primed(), new SequentialCommandGroup(
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.LEFT)
)),
// new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
// new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.LEFT),
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.LEFT),
new waitEndefectorRefrence(m_robotElevator),
new waitElevatorRefrence(m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_1, Side.LEFT),
@@ -161,17 +174,21 @@ public class RobotContainer {
new Translation2d(0,1), new Translation2d(), 500, true),
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator)
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator),
new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
);
private Command AprilLidarAlignL3Left = new SequentialCommandGroup(
new InstantCommand(() -> {m_robotSwerveDrive.shiftDown();m_robotSwerveDrive.shiftDown();}),
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedThree), m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L3_DISTANCE_2, Side.LEFT),
// new InstantCommand(() -> System.out.println("Soup")),
// new WaitCommand(1),
new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
new IfCommand(() -> m_robotElevator.isL3Primed(), new SequentialCommandGroup(
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedThree), m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L3_DISTANCE_2, Side.LEFT)
)),
new waitEndefectorRefrence(m_robotElevator),
new waitElevatorRefrence(m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L3_DISTANCE_1 - Units.inchesToMeters(0), Side.LEFT),
new LidarAlign(m_robotSwerveDrive, m_lidar),
// new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringThree), m_robotElevator),
@@ -179,13 +196,19 @@ public class RobotContainer {
new waitElevatorRefrence(m_robotElevator),
// new MoveForTimeCommand(m_robotSwerveDrive,
// new Translation2d(0,1), new Translation2d(), 500, true),
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator)
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator),
new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
);
private Command AprilLidarAlignL3Right = new SequentialCommandGroup(
new InstantCommand(() -> {m_robotSwerveDrive.shiftDown();m_robotSwerveDrive.shiftDown();}),
new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
new IfCommand(() -> m_robotElevator.isL3Primed(), new SequentialCommandGroup(
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedThree), m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L3_DISTANCE_2, Side.RIGHT)
)),
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedThree), m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L3_DISTANCE_2, Side.RIGHT),
new waitEndefectorRefrence(m_robotElevator),
@@ -201,11 +224,12 @@ public class RobotContainer {
new waitElevatorRefrence(m_robotElevator),
// new MoveForTimeCommand(m_robotSwerveDrive,
// new Translation2d(0,1), new Translation2d(), 500, true),
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator)
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator),
new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
);
private Command AprilLidarAlignL2Left = new SequentialCommandGroup(
new InstantCommand(() -> {m_robotSwerveDrive.shiftDown();m_robotSwerveDrive.shiftDown();}),
new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L2_SCORE_DISTANCE, Side.LEFT),
new LidarAlign(m_robotSwerveDrive, m_lidar),
@@ -214,12 +238,13 @@ public class RobotContainer {
new waitElevatorRefrence(m_robotElevator),
new MoveForTimeCommand(m_robotSwerveDrive,
new Translation2d(0,1), new Translation2d(), 500, true),
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator)
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator),
new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
);
private Command AprilLidarAlignL2Right = new SequentialCommandGroup(
new InstantCommand(() -> {m_robotSwerveDrive.shiftDown();m_robotSwerveDrive.shiftDown();}),
new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L2_SCORE_DISTANCE, Side.RIGHT),
new LidarAlign(m_robotSwerveDrive, m_lidar),
@@ -228,8 +253,8 @@ public class RobotContainer {
new waitElevatorRefrence(m_robotElevator),
new MoveForTimeCommand(m_robotSwerveDrive,
new Translation2d(0,1), new Translation2d(), 500, true),
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator)
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator),
new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
);
private Command placeCoral = new SequentialCommandGroup(
@@ -251,7 +276,7 @@ public class RobotContainer {
);
private Command leftAlgaeRemove = new SequentialCommandGroup(
new InstantCommand(() -> {m_robotSwerveDrive.shiftDown();m_robotSwerveDrive.shiftDown();}),
new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL2Primed);}, m_robotElevator),
new waitEndefectorRefrence(m_robotElevator),
new waitElevatorRefrence(m_robotElevator),
@@ -260,11 +285,12 @@ public class RobotContainer {
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL2Go);}, m_robotElevator),
new MoveForTimeCommand(m_robotSwerveDrive,
new Translation2d(0,1), new Translation2d(), 500, true),
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator)
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator),
new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
);
private Command rightAlgaeRemove = new SequentialCommandGroup(
new InstantCommand(() -> {m_robotSwerveDrive.shiftDown();m_robotSwerveDrive.shiftDown();}),
new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL3Primed);}, m_robotElevator),
new waitEndefectorRefrence(m_robotElevator),
new waitElevatorRefrence(m_robotElevator),
@@ -273,7 +299,8 @@ public class RobotContainer {
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL3Go);}, m_robotElevator),
new MoveForTimeCommand(m_robotSwerveDrive,
new Translation2d(0,1), new Translation2d(), 500, true),
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator)
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator),
new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
);
@@ -299,7 +326,12 @@ public class RobotContainer {
configureButtonBindings();
configureVirtualButtonBindings();
new DeferredBlock(() -> m_robotSwerveDrive.resetGyro());
new DeferredBlock(() -> { // Called on first robot enable
m_robotSwerveDrive.resetGyro();
});
new DeferredBlockMulti(() -> { // Called on every robot enable
TimesNegativeOne.update();
});
DriverStation.silenceJoystickConnectionWarning(true);
// CameraServer.startAutomaticCapture();
@@ -449,7 +481,11 @@ public class RobotContainer {
// Cancel button
new JoystickButton(getButtonBox(), ButtonBox.White)
.onTrue(new InstantCommand(() -> {m_robotElevator.elevatorStop(); m_robotElevator.endefectorStop();}, m_robotElevator));
.onTrue(new InstantCommand(() -> {
m_robotElevator.elevatorStop();
m_robotElevator.endefectorStop();
m_robotSwerveDrive.endSlowPeriod();
}, m_robotElevator));
// Score prep
// Prime 4
@@ -469,7 +505,6 @@ public class RobotContainer {
new Trigger(() -> getDeadbandedOperatorController().getPOV() == 180)
.onTrue(new InstantCommand(() -> AutoConstants.ELEVATOR_OFFSET_TRIM.stepDown()));
new Trigger(() -> getDeadbandedOperatorController().getPOV() == 90)
.onTrue(new InstantCommand(() -> AutoConstants.ARM_OFFSET_TRIM.stepUp()));
src/main/java/frc4388/robot/commands/GotoLastApril.java
+19 -11
View File
@@ -15,6 +15,7 @@ import frc4388.robot.subsystems.SwerveDrive;
import frc4388.robot.subsystems.Vision;
import frc4388.utility.Gains;
import frc4388.utility.ReefPositionHelper;
import frc4388.utility.TimesNegativeOne;
import frc4388.utility.ReefPositionHelper.Side;
import frc4388.utility.UtilityStructs.AutoRecordingControllerFrame;
import frc4388.utility.UtilityStructs.AutoRecordingFrame;
@@ -75,19 +76,26 @@ public class GotoLastApril extends Command {
@Override
public void execute() {
if (alliance == Alliance.Red) {
xerr = -(targetpos.getX() - vision.getPose2d().getX());
yerr = (targetpos.getY() - vision.getPose2d().getY());
}
else{
xerr = targetpos.getX() - vision.getPose2d().getX();
yerr = targetpos.getY() - vision.getPose2d().getY();
xerr = TimesNegativeOne.invert(targetpos.getX() - vision.getPose2d().getX(), TimesNegativeOne.XAxis);
yerr = TimesNegativeOne.invert(targetpos.getY() - vision.getPose2d().getY(), !TimesNegativeOne.YAxis);
// xerr = targetpos.getX() - vision.getPose2d().getX();
// yerr = targetpos.getX() - vision.getPose2d().getY();
// roterr = TimesNegativeOne.invert(targetpos.getRotation().getDegrees() - vision.getPose2d().getRotation().getDegrees(), TimesNegativeOne.isRed);
roterr = ((targetpos.getRotation().getDegrees() - vision.getPose2d().getRotation().getDegrees()));
boolean invert = Math.abs(roterr) > 180;
if(roterr > 180){
roterr -= 360;
}else if(roterr < -180){
roterr += 360;
}
roterr = targetpos.getRotation().getDegrees() - vision.getPose2d().getRotation().getDegrees();
SmartDashboard.putNumber("Rotational PID target", targetpos.getRotation().getDegrees());
SmartDashboard.putNumber("Rotational PID position", vision.getPose2d().getRotation().getDegrees());
SmartDashboard.putNumber("Rotational PID error", roterr);
// SmartDashboard.putNumber("PID X Error", xerr);
// SmartDashboard.putNumber("PID Y Error", yerr);
src/main/java/frc4388/robot/commands/IfCommand.java
+33
View File
@@ -0,0 +1,33 @@
package frc4388.robot.commands;
import java.time.Instant;
import java.util.function.BooleanSupplier;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.InstantCommand;
// Command that is called if something is true
public class IfCommand extends InstantCommand {
BooleanSupplier isTrue;
Command trueCommand;
Command falseCommand;
public IfCommand(BooleanSupplier isTrue, Command trueCommand, Command falseCommand){
this.isTrue = isTrue;
this.trueCommand = trueCommand;
this.falseCommand = falseCommand;
}
public IfCommand(BooleanSupplier isTrue, Command trueCommand){
this(isTrue, trueCommand, Commands.none());
}
@Override
public void execute() {
if(isTrue.getAsBoolean())
trueCommand.schedule();
else
falseCommand.schedule();
}
}
src/main/java/frc4388/robot/commands/LidarAlign.java
+11 -1
View File
@@ -21,6 +21,7 @@ public class LidarAlign extends Command {
private boolean foundReef;
private boolean headedRight;
private double speed;
private int bounces;
// private final boolean constructedHeadedRight;
/** Creates a new LidarAlign. */
@@ -40,6 +41,7 @@ public class LidarAlign extends Command {
this.speed = 0.4; // TODO: find good speed for this
this.foundReef = false;
this.headedRight = !(GotoLastApril.tagRelativeXError < 0);
this.bounces = 0;
}
@@ -55,6 +57,8 @@ public class LidarAlign extends Command {
if (currentFinderTick > 10) { //arbutrary threshhold for now.
headedRight = !headedRight;
currentFinderTick *= -1;
bounces++;
if (bounces == 2) return;
}
double currentHeading = (swerveDrive.getGyroAngle() * 180) / Math.PI;
double relAngle = (Math.round(currentHeading / 60.d) * 60); // Relative driving to the side of the reef
@@ -78,7 +82,10 @@ public class LidarAlign extends Command {
// Returns true when the command should end.
@Override
public boolean isFinished() {
if (foundReef && lidar.withinDistance()) { // spot on
if (lidar.getDistance() < 4) {
swerveDrive.stopModules();
return true;
} else if (foundReef && lidar.withinDistance()) { // spot on
swerveDrive.stopModules();
return true;
} else if (foundReef && !lidar.withinDistance()) { // over shot
@@ -87,6 +94,9 @@ public class LidarAlign extends Command {
currentFinderTick = 0;
foundReef = false;
return false;
} else if (bounces == 2) {
swerveDrive.stopModules();
return true;
} else {
return false;
}
src/main/java/frc4388/robot/commands/MoveForTimeCommand.java
+1
View File
@@ -5,6 +5,7 @@ import java.time.Instant;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj2.command.Command;
import frc4388.robot.subsystems.SwerveDrive;
import frc4388.utility.TimesNegativeOne;
// Command to repeat a joystick movement for a specific time.
public class MoveForTimeCommand extends Command {
src/main/java/frc4388/robot/subsystems/Elevator.java
+8
View File
@@ -243,6 +243,14 @@ public class Elevator extends SubsystemBase {
// }
}
public boolean isL4Primed(){
return currentState == CoordinationState.PrimedFour;
}
public boolean isL3Primed(){
return currentState == CoordinationState.PrimedThree;
}
public void armShuffle(){
if(!basinBeamBreak.get()){
//shuffle the coral with the arm until coral hits beam break
src/main/java/frc4388/robot/subsystems/Lidar.java
+1 -1
View File
@@ -72,7 +72,7 @@ public class Lidar extends Subsystem {
if(distance == -1){
s.addReport(ReportLevel.ERROR, "LIDAR DISCONNECTED");
}else{
s.addReport(ReportLevel.ERROR, "LIDAR CONNECTED");
s.addReport(ReportLevel.INFO, "LIDAR CONNECTED");
}
return s;
}
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+25 -23
View File
@@ -35,9 +35,8 @@ import frc4388.robot.commands.GotoLastApril;
import frc4388.robot.commands.LidarAlign;
import frc4388.utility.Status;
import frc4388.utility.Subsystem;
import frc4388.utility.TimesNegativeOne;
import frc4388.utility.Status.ReportLevel;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import com.pathplanner.lib.controllers.PPHolonomicDriveController;
import com.pathplanner.lib.config.PIDConstants;
@@ -103,11 +102,11 @@ public class SwerveDrive extends Subsystem {
// This will flip the path being followed to the red side of the field.
// THE ORIGIN WILL REMAIN ON THE BLUE SIDE
var alliance = DriverStation.getAlliance();
if (alliance.isPresent()) {
return alliance.get() == DriverStation.Alliance.Red;
}
return false;
// var alliance = DriverStation.getAlliance();
// if (alliance.isPresent()) {
// return alliance.get() == DriverStation.Alliance.Red;
// }
return TimesNegativeOne.isRed;
},
this // Reference to this subsystem to set requirements
);
@@ -140,23 +139,14 @@ public class SwerveDrive extends Subsystem {
if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05) // if no imput
return; // don't bother doing swerve drive math and return early.
leftStick = leftStick.rotateBy(Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET));
Optional<Alliance> alliance = DriverStation.getAlliance();
if(!alliance.isEmpty()){
if (alliance.get() == Alliance.Red)
leftStick = new Translation2d(-leftStick.getX(), leftStick.getY());
else
leftStick = new Translation2d(leftStick.getX(), -leftStick.getY());
// if (alliance.get() != Alliance.Red) leftStick = new Translation2d(leftStick.getX(), -leftStick.getY());
}
if (SwerveDriveConstants.INVERT_ROTATION) rightStick.times(-1);
leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset);
stopped = false;
if (fieldRelative) {
leftStick = TimesNegativeOne.invert(leftStick, TimesNegativeOne.XAxis, TimesNegativeOne.YAxis);
rightStick = TimesNegativeOne.invert(rightStick, TimesNegativeOne.RotAxis);
// ! drift correction
if (rightStick.getNorm() > 0.05 || !SwerveDriveConstants.DRIFT_CORRECTION_ENABLED) {
rotTarget = swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds()).orElse(new Pose2d()).getRotation().getDegrees();
@@ -218,7 +208,7 @@ public class SwerveDrive extends Subsystem {
if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05) // if no imput
return; // don't bother doing swerve drive math and return early.
leftStick.rotateBy(Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET));
leftStick.rotateBy(TimesNegativeOne.ForwardOffset);
swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle()
.withVelocityX(leftStick.getX() * speedAdjust)
@@ -282,7 +272,7 @@ public class SwerveDrive extends Subsystem {
// if (leftStick.getNorm() < 0.05) //if no imput
// return; // don't bother doing swerve drive math and return early.
leftStick = leftStick.rotateBy(Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET));
leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset);
swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle()
.withVelocityX(leftStick.getX() * -speedAdjust)
@@ -390,6 +380,18 @@ public class SwerveDrive extends Subsystem {
rotSpeedAdjust = SwerveDriveConstants.MIN_ROT_SPEED;
}
private int tmp_gear_index = SwerveDriveConstants.STARTING_GEAR;
public void startSlowPeriod() {
tmp_gear_index = gear_index;
setToSlow();
}
public void endSlowPeriod() {
setPercentOutput(SwerveDriveConstants.GEARS[tmp_gear_index]);
gear_index = tmp_gear_index;
}
@Override
public String getSubsystemName() {
return "Swerve Drive Controller";
src/main/java/frc4388/utility/Alliance.java
+5
View File
@@ -0,0 +1,5 @@
package frc4388.utility;
public class Alliance {
}
src/main/java/frc4388/utility/DeferredBlockMulti.java
+18
View File
@@ -0,0 +1,18 @@
package frc4388.utility;
import java.util.ArrayList;
public class DeferredBlockMulti {
private static ArrayList<Runnable> m_blocks = new ArrayList<>();
public DeferredBlockMulti(Runnable block) {
m_blocks.add(block);
}
public static void execute() {
for (Runnable block : m_blocks) {
block.run();
}
}
}
src/main/java/frc4388/utility/ReefPositionHelper.java
+4 -7
View File
@@ -63,14 +63,11 @@ public class ReefPositionHelper {
* Function to find closest tag location based on side
*/
public static Pose2d getNearestTag(Pose2d position) {
Optional<Alliance> ally = DriverStation.getAlliance();
if (!ally.isPresent())
return new Pose2d();
if (ally.get() == Alliance.Red)
if(TimesNegativeOne.isRed)
return getNearestTag(RED_TAGS, position);
if (ally.get() == Alliance.Blue)
return getNearestTag(BLUE_TAGS, position);
return new Pose2d();
else
return getNearestTag(BLUE_TAGS, position);
}
public static Pose2d getNearestPosition(Pose2d position, Side side, double xtrim, double ydistance) {
src/main/java/frc4388/utility/TimesNegativeOne.java
+51
View File
@@ -0,0 +1,51 @@
package frc4388.utility;
import java.util.Optional;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import frc4388.robot.Constants.SwerveDriveConstants;
// Class that holds weather the drivers sticks should be inverted
public class TimesNegativeOne {
public static boolean XAxis = SwerveDriveConstants.INVERT_X;
public static boolean YAxis = SwerveDriveConstants.INVERT_Y;
public static boolean RotAxis = SwerveDriveConstants.INVERT_ROTATION;
public static boolean isRed = false;
public static Rotation2d ForwardOffset = Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET);
private static boolean isRed() {
Optional<Alliance> alliance = DriverStation.getAlliance();
if(alliance.isEmpty()) return false;
return (alliance.get() == Alliance.Red);
}
public static void update(){
isRed = isRed();
XAxis = SwerveDriveConstants.INVERT_X ^ isRed;
YAxis = SwerveDriveConstants.INVERT_Y ^ isRed;
RotAxis = SwerveDriveConstants.INVERT_ROTATION;
ForwardOffset = Rotation2d.fromDegrees((SwerveDriveConstants.FORWARD_OFFSET + (isRed ? 0 : 0)));
SmartDashboard.putBoolean("Is red alliance", isRed);
}
public static double invert(double num, boolean invert){
return invert ? -num : num;
}
public static Translation2d invert(Translation2d stick, boolean invertXY){
if(invertXY) return stick.times(-1);
else return stick;
}
public static Translation2d invert(Translation2d stick, boolean invertX, boolean invertY){
return new Translation2d(
invert(stick.getX(), invertX),
invert(stick.getY(), invertY)
);
}
}
vendordeps/PathplannerLib-2025.1.1.json → vendordeps/PathplannerLib-2025.2.4.json
+4 -4
View File
@@ -1,7 +1,7 @@
{
"fileName": "PathplannerLib-2025.1.1.json",
"fileName": "PathplannerLib-2025.2.4.json",
"name": "PathplannerLib",
"version": "2025.1.1",
"version": "2025.2.4",
"uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786",
"frcYear": "2025",
"mavenUrls": [
@@ -12,7 +12,7 @@
{
"groupId": "com.pathplanner.lib",
"artifactId": "PathplannerLib-java",
"version": "2025.1.1"
"version": "2025.2.4"
}
],
"jniDependencies": [],
@@ -20,7 +20,7 @@
{
"groupId": "com.pathplanner.lib",
"artifactId": "PathplannerLib-cpp",
"version": "2025.1.1",
"version": "2025.2.4",
"libName": "PathplannerLib",
"headerClassifier": "headers",
"sharedLibrary": false,