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

Work on Autos

Browse Source
This commit is contained in:
Michael Mikovsky
2025-03-05 19:42:36 -07:00
parent aeedebd095
commit a6daadb537
15 changed files with 191 additions and 93 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/deploy/pathplanner/autos/3 Coral Cage 4.auto
+85
View File
@@ -0,0 +1,85 @@
{
"version": "2025.0",
"command": {
"type": "sequential",
"data": {
"commands": [
{
"type": "named",
"data": {
"name": "prepare-l4"
}
},
{
"type": "path",
"data": {
"pathName": "Cage 4 to Reef"
}
},
{
"type": "named",
"data": {
"name": "place-coral-left-l4"
}
},
{
"type": "path",
"data": {
"pathName": "Reef 3 to Bottom Feed"
}
},
{
"type": "named",
"data": {
"name": "grab-coral"
}
},
{
"type": "path",
"data": {
"pathName": "Bottom Feed to Reef 2"
}
},
{
"type": "named",
"data": {
"name": "place-coral-right-l4"
}
},
{
"type": "path",
"data": {
"pathName": "Reef 2 to Bottom Feed"
}
},
{
"type": "named",
"data": {
"name": "grab-coral"
}
},
{
"type": "path",
"data": {
"pathName": "Bottom Feed to Reef 2"
}
},
{
"type": "named",
"data": {
"name": "place-coral-left-l4"
}
},
{
"type": "path",
"data": {
"pathName": "Reef 2 to Bottom Feed"
}
}
]
}
},
"resetOdom": true,
"folder": null,
"choreoAuto": false
}
src/main/deploy/pathplanner/autos/3 Coral Cage 5.auto
-12
View File
@@ -28,12 +28,6 @@
"pathName": "Reef 3 to Bottom Feed" "pathName": "Reef 3 to Bottom Feed"
} }
}, },
{
"type": "named",
"data": {
"name": "align-feed"
}
},
{ {
"type": "named", "type": "named",
"data": { "data": {
@@ -58,12 +52,6 @@
"pathName": "Reef 2 to Bottom Feed" "pathName": "Reef 2 to Bottom Feed"
} }
}, },
{
"type": "named",
"data": {
"name": "align-feed"
}
},
{ {
"type": "named", "type": "named",
"data": { "data": {
src/main/deploy/pathplanner/paths/Bottom Feed to Reef 2.path
+4 -4
View File
@@ -3,13 +3,13 @@
"waypoints": [ "waypoints": [
{ {
"anchor": { "anchor": {
"x": 1.0603125, "x": 1.0807449494949497,
"y": 1.0390625 "y": 1.0875631313131315
}, },
"prevControl": null, "prevControl": null,
"nextControl": { "nextControl": {
"x": 2.3463856916648673, "x": 2.3668181411598166,
"y": 1.797152889765366 "y": 1.8456535210784974
}, },
"isLocked": false, "isLocked": false,
"linkedName": null "linkedName": null
src/main/deploy/pathplanner/paths/Cage 4 to Reef.path
+7 -7
View File
@@ -3,13 +3,13 @@
"waypoints": [ "waypoints": [
{ {
"anchor": { "anchor": {
"x": 7.595959595959596, "x": 7.006439393939393,
"y": 3.0289141414141416 "y": 3.1508838383838382
}, },
"prevControl": null, "prevControl": null,
"nextControl": { "nextControl": {
"x": 6.2521875, "x": 6.498232323232323,
"y": 2.6234375000000005 "y": 2.480050505050505
}, },
"isLocked": false, "isLocked": false,
"linkedName": null "linkedName": null
@@ -20,8 +20,8 @@
"y": 2.6934974747474745 "y": 2.6934974747474745
}, },
"prevControl": { "prevControl": {
"x": 5.630625, "x": 5.979861111111112,
"y": 2.148125 "y": 1.992171717171717
}, },
"nextControl": null, "nextControl": null,
"isLocked": false, "isLocked": false,
@@ -48,7 +48,7 @@
"folder": "Barge to Reef", "folder": "Barge to Reef",
"idealStartingState": { "idealStartingState": {
"velocity": 0, "velocity": 0,
"rotation": -178.9832812445108 "rotation": 124.11447294534119
}, },
"useDefaultConstraints": true "useDefaultConstraints": true
} }
src/main/deploy/pathplanner/paths/Cage 5 to Reef.path
+10 -10
View File
@@ -3,25 +3,25 @@
"waypoints": [ "waypoints": [
{ {
"anchor": { "anchor": {
"x": 7.129687499999999, "x": 7.016603535353535,
"y": 1.9409375 "y": 1.9718434343434341
}, },
"prevControl": null, "prevControl": null,
"nextControl": { "nextControl": {
"x": 6.072698863636364, "x": 6.000189393939394,
"y": 1.0434943181818186 "y": 1.6364267676767676
}, },
"isLocked": false, "isLocked": false,
"linkedName": null "linkedName": null
}, },
{ {
"anchor": { "anchor": {
"x": 5.2771875, "x": 5.47165404040404,
"y": 2.61125 "y": 2.1141414141414145
}, },
"prevControl": { "prevControl": {
"x": 5.504318181818181, "x": 5.86940340909091,
"y": 2.1902272727272725 "y": 1.7593308080808083
}, },
"nextControl": null, "nextControl": null,
"isLocked": false, "isLocked": false,
@@ -42,13 +42,13 @@
}, },
"goalEndState": { "goalEndState": {
"velocity": 0, "velocity": 0,
"rotation": 118.99790479482637 "rotation": 124.8244891569568
}, },
"reversed": false, "reversed": false,
"folder": "Barge to Reef", "folder": "Barge to Reef",
"idealStartingState": { "idealStartingState": {
"velocity": 0, "velocity": 0,
"rotation": 179.5295681977034 "rotation": 126.34745820888533
}, },
"useDefaultConstraints": true "useDefaultConstraints": true
} }
src/main/deploy/pathplanner/paths/Cage 6 to Reef.path
+7 -7
View File
@@ -3,13 +3,13 @@
"waypoints": [ "waypoints": [
{ {
"anchor": { "anchor": {
"x": 7.565467171717171, "x": 7.057260101010102,
"y": 0.8334595959595947 "y": 0.8537878787878788
}, },
"prevControl": null, "prevControl": null,
"nextControl": { "nextControl": {
"x": 6.625889381464077, "x": 6.315277777777778,
"y": 1.4477106264692527 "y": 1.5144570707070704
}, },
"isLocked": false, "isLocked": false,
"linkedName": null "linkedName": null
@@ -20,8 +20,8 @@
"y": 2.6234375 "y": 2.6234375
}, },
"prevControl": { "prevControl": {
"x": 5.721332938841585, "x": 5.624116161616161,
"y": 2.265481302925542 "y": 2.205618686868687
}, },
"nextControl": null, "nextControl": null,
"isLocked": false, "isLocked": false,
@@ -48,7 +48,7 @@
"folder": "Barge to Reef", "folder": "Barge to Reef",
"idealStartingState": { "idealStartingState": {
"velocity": 0, "velocity": 0,
"rotation": -179.94258861633824 "rotation": 132.5633517531898
}, },
"useDefaultConstraints": true "useDefaultConstraints": true
} }
src/main/deploy/pathplanner/paths/Reef 2 to Bottom Feed.path
+11 -11
View File
@@ -3,25 +3,25 @@
"waypoints": [ "waypoints": [
{ {
"anchor": { "anchor": {
"x": 3.680625, "x": 3.749318181818182,
"y": 2.61125 "y": 2.868295454545454
}, },
"prevControl": null, "prevControl": null,
"nextControl": { "nextControl": {
"x": 3.2541591082344743, "x": 3.093244949494949,
"y": 2.3264921263556886 "y": 2.2869318181818175
}, },
"isLocked": false, "isLocked": false,
"linkedName": null "linkedName": null
}, },
{ {
"anchor": { "anchor": {
"x": 1.12125, "x": 1.3246843434343436,
"y": 1.0025 "y": 1.5347853535353522
}, },
"prevControl": { "prevControl": {
"x": 1.3989083079519464, "x": 1.802550505050505,
"y": 1.1682576997602765 "y": 2.1992045454545446
}, },
"nextControl": null, "nextControl": null,
"isLocked": false, "isLocked": false,
@@ -33,8 +33,8 @@
"pointTowardsZones": [], "pointTowardsZones": [],
"eventMarkers": [], "eventMarkers": [],
"globalConstraints": { "globalConstraints": {
"maxVelocity": 3.0, "maxVelocity": 5.0,
"maxAcceleration": 3.0, "maxAcceleration": 5.0,
"maxAngularVelocity": 540.0, "maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0, "maxAngularAcceleration": 720.0,
"nominalVoltage": 12.0, "nominalVoltage": 12.0,
@@ -50,5 +50,5 @@
"velocity": 0, "velocity": 0,
"rotation": 58.10920819815426 "rotation": 58.10920819815426
}, },
"useDefaultConstraints": true "useDefaultConstraints": false
} }
src/main/deploy/pathplanner/paths/Reef 3 to Bottom Feed.path
+11 -11
View File
@@ -3,25 +3,25 @@
"waypoints": [ "waypoints": [
{ {
"anchor": { "anchor": {
"x": 5.265, "x": 5.268371212121212,
"y": 2.61125 "y": 2.4597222222222217
}, },
"prevControl": null, "prevControl": null,
"nextControl": { "nextControl": {
"x": 4.137208815063533, "x": 4.670075757575758,
"y": 2.0700494801809173 "y": 1.2531597222222217
}, },
"isLocked": false, "isLocked": false,
"linkedName": null "linkedName": null
}, },
{ {
"anchor": { "anchor": {
"x": 1.096875, "x": 1.1010732323232322,
"y": 1.0390625 "y": 1.00625
}, },
"prevControl": { "prevControl": {
"x": 1.3299317623663032, "x": 1.786268939393939,
"y": 1.1501468126694903 "y": 2.2439551767676766
}, },
"nextControl": null, "nextControl": null,
"isLocked": false, "isLocked": false,
@@ -33,8 +33,8 @@
"pointTowardsZones": [], "pointTowardsZones": [],
"eventMarkers": [], "eventMarkers": [],
"globalConstraints": { "globalConstraints": {
"maxVelocity": 3.0, "maxVelocity": 5.0,
"maxAcceleration": 3.0, "maxAcceleration": 5.0,
"maxAngularVelocity": 540.0, "maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0, "maxAngularAcceleration": 720.0,
"nominalVoltage": 12.0, "nominalVoltage": 12.0,
@@ -50,5 +50,5 @@
"velocity": 0, "velocity": 0,
"rotation": 121.42956561483854 "rotation": 121.42956561483854
}, },
"useDefaultConstraints": true "useDefaultConstraints": false
} }
src/main/deploy/pathplanner/paths/Reef 4 to Bottom Feed.path
+1 -1
View File
@@ -20,7 +20,7 @@
"y": 1.0146875 "y": 1.0146875
}, },
"prevControl": { "prevControl": {
"x": 1.95, "x": 1.9500000000000002,
"y": 2.1115625000000002 "y": 2.1115625000000002
}, },
"nextControl": null, "nextControl": null,
src/main/java/frc4388/robot/Constants.java
+4 -4
View File
@@ -193,7 +193,7 @@ public final class Constants {
.withKS(0).withKV(0.124); .withKS(0).withKV(0.124);
public static final Gains DRIFT_CORRECTION_GAINS = new Gains(2.5, 0, 0.1); public static final Gains DRIFT_CORRECTION_GAINS = new Gains(2.5, 0, 0.1);
public static final Gains RELATIVE_LOCKED_ANGLE_GAINS = new Gains(5, 0, 0.1); // TODO: TEST public static final Gains RELATIVE_LOCKED_ANGLE_GAINS = new Gains(5, 0.2, 0.1); // TODO: TEST
} }
public static final class Configurations { public static final class Configurations {
@@ -294,7 +294,7 @@ public final class Constants {
} }
public static final class AutoConstants { public static final class AutoConstants {
public static final Gains XY_GAINS = new Gains(5,0.4,0.0); public static final Gains XY_GAINS = new Gains(5,0.6,0.0);
// public static final Gains XY_GAINS = new Gains(3,0.3,0.0); // public static final Gains XY_GAINS = new Gains(3,0.3,0.0);
// public static final Gains ROT_GAINS = new Gains(0.05,0,0.007); // public static final Gains ROT_GAINS = new Gains(0.05,0,0.007);
@@ -316,10 +316,10 @@ public final class Constants {
public static final double X_SCORING_POSITION_OFFSET = Units.inchesToMeters(6.5); // This is from the field public static final double X_SCORING_POSITION_OFFSET = Units.inchesToMeters(6.5); // This is from the field
public static final double Y_SCORING_POSITION_OFFSET = Units.inchesToMeters(16); public static final double Y_SCORING_POSITION_OFFSET = Units.inchesToMeters(16);
public static final double L4_DISTANCE_PREP = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(12); public static final double L4_DISTANCE_PREP = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(15);
public static final double L4_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(5.5); public static final double L4_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(5.5);
public static final double L3_DISTANCE_PREP = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(12); public static final double L3_DISTANCE_PREP = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(15);
public static final double L3_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(6); public static final double L3_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(6);
public static final double L2_PREP_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(6); public static final double L2_PREP_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(6);
src/main/java/frc4388/robot/RobotContainer.java
+20 -13
View File
@@ -317,12 +317,19 @@ public class RobotContainer {
private Boolean operatorManualMode = false; private Boolean operatorManualMode = false;
public Command LoopAprilLidarAlignL4Left = new WhileTrueCommand(AprilLidarAlignL4Left, () -> !m_robotElevator.hasCoral()); // public Command LoopAprilLidarAlignL4Left = new WhileTrueCommand(AprilLidarAlignL4Left.asProxy(), () -> !m_robotElevator.hasCoral());
public Command LoopAprilLidarAlignL4Right = new WhileTrueCommand(AprilLidarAlignL4Right, () -> !m_robotElevator.hasCoral()); // public Command LoopAprilLidarAlignL4Right = new WhileTrueCommand(AprilLidarAlignL4Right.asProxy(), () -> !m_robotElevator.hasCoral());
public Command LoopAprilLidarAlignL3Left = new WhileTrueCommand(AprilLidarAlignL3Left, () -> !m_robotElevator.hasCoral()); // public Command LoopAprilLidarAlignL3Left = new WhileTrueCommand(AprilLidarAlignL3Left.asProxy(), () -> !m_robotElevator.hasCoral());
public Command LoopAprilLidarAlignL3Right = new WhileTrueCommand(AprilLidarAlignL3Right, () -> !m_robotElevator.hasCoral()); // public Command LoopAprilLidarAlignL3Right = new WhileTrueCommand(AprilLidarAlignL3Right.asProxy(), () -> !m_robotElevator.hasCoral());
public Command LoopAprilLidarAlignL2Left = new WhileTrueCommand(AprilLidarAlignL2Left, () -> !m_robotElevator.hasCoral()); // public Command LoopAprilLidarAlignL2Left = new WhileTrueCommand(AprilLidarAlignL2Left.asProxy(), () -> !m_robotElevator.hasCoral());
public Command LoopAprilLidarAlignL2Right = new WhileTrueCommand(AprilLidarAlignL2Right, () -> !m_robotElevator.hasCoral()); // public Command LoopAprilLidarAlignL2Right = new WhileTrueCommand(AprilLidarAlignL2Right.asProxy(), () -> !m_robotElevator.hasCoral());
// public Command LoopAprilLidarAlignL4Left = new SequentialCommandGroup(AprilLidarAlignL4Left.asProxy(), new ConditionalCommand(AprilLidarAlignL4Left.asProxy(), Commands.none(), () -> !m_robotElevator.hasCoral()));
// public Command LoopAprilLidarAlignL4Right = new SequentialCommandGroup(AprilLidarAlignL4Right.asProxy(), new ConditionalCommand(AprilLidarAlignL4Right.asProxy(), Commands.none(), () -> !m_robotElevator.hasCoral()));
// public Command LoopAprilLidarAlignL3Left = new SequentialCommandGroup(AprilLidarAlignL3Left.asProxy(), new ConditionalCommand(AprilLidarAlignL3Left.asProxy(), Commands.none(), () -> m_robotElevator.hasCoral()));
// public Command LoopAprilLidarAlignL3Right = new SequentialCommandGroup(AprilLidarAlignL3Right.asProxy(), new ConditionalCommand(AprilLidarAlignL3Right.asProxy(), Commands.none(), () -> m_robotElevator.hasCoral()));
// public Command LoopAprilLidarAlignL2Left = new SequentialCommandGroup(AprilLidarAlignL2Left.asProxy(), new ConditionalCommand(AprilLidarAlignL2Left.asProxy(), Commands.none(), () -> m_robotElevator.hasCoral()));
// public Command LoopAprilLidarAlignL2Right = new SequentialCommandGroup(AprilLidarAlignL2Right.asProxy(), new ConditionalCommand(AprilLidarAlignL2Right.asProxy(), Commands.none(), () -> m_robotElevator.hasCoral()));
/** /**
* The container for the robot. Contains subsystems, OI devices, and commands. * The container for the robot. Contains subsystems, OI devices, and commands.
*/ */
@@ -334,15 +341,15 @@ public class RobotContainer {
NamedCommands.registerCommand("align-feed", new SequentialCommandGroup( NamedCommands.registerCommand("align-feed", new SequentialCommandGroup(
new MoveForTimeCommand(m_robotSwerveDrive, new MoveForTimeCommand(m_robotSwerveDrive,
new Translation2d(0, 1), new Translation2d(0, 1),
new Translation2d(), 400, true new Translation2d(), 300, true
), new InstantCommand(()-> {m_robotSwerveDrive.softStop();} , m_robotSwerveDrive))); ), new InstantCommand(()-> {m_robotSwerveDrive.softStop();} , m_robotSwerveDrive)));
NamedCommands.registerCommand("place-coral-left-l4", LoopAprilLidarAlignL4Left); NamedCommands.registerCommand("place-coral-left-l4", AprilLidarAlignL4Left);
NamedCommands.registerCommand("place-coral-right-l4", LoopAprilLidarAlignL4Right); NamedCommands.registerCommand("place-coral-right-l4", AprilLidarAlignL4Right);
NamedCommands.registerCommand("place-coral-left-l3", LoopAprilLidarAlignL3Left); NamedCommands.registerCommand("place-coral-left-l3", AprilLidarAlignL3Left);
NamedCommands.registerCommand("place-coral-right-l3", LoopAprilLidarAlignL3Right); NamedCommands.registerCommand("place-coral-right-l3", AprilLidarAlignL3Right);
NamedCommands.registerCommand("place-coral-left-l2", LoopAprilLidarAlignL2Left); NamedCommands.registerCommand("place-coral-left-l2", AprilLidarAlignL2Left);
NamedCommands.registerCommand("place-coral-right-l2", LoopAprilLidarAlignL2Right); NamedCommands.registerCommand("place-coral-right-l2", AprilLidarAlignL2Right);
NamedCommands.registerCommand("prepare-l4", new InstantCommand(() -> { NamedCommands.registerCommand("prepare-l4", new InstantCommand(() -> {
m_robotElevator.transitionState(CoordinationState.PrimedFour); m_robotElevator.transitionState(CoordinationState.PrimedFour);
src/main/java/frc4388/robot/commands/GotoLastApril.java
+7 -6
View File
@@ -95,12 +95,13 @@ public class GotoLastApril extends Command {
roterr += 360; roterr += 360;
} }
SmartDashboard.putNumber("Rotational PID target", targetpos.getRotation().getDegrees()); // SmartDashboard.putNumber("Rotational PID target", targetpos.getRotation().getDegrees());
SmartDashboard.putNumber("Rotational PID position", vision.getPose2d().getRotation().getDegrees()); // SmartDashboard.putNumber("Rotational PID position", vision.getPose2d().getRotation().getDegrees());
SmartDashboard.putNumber("Rotational PID error", roterr); // SmartDashboard.putNumber("Rotational PID error", roterr);
// SmartDashboard.putNumber("PID X Error", xerr); SmartDashboard.putNumber("PID X Error", xerr);
// SmartDashboard.putNumber("PID Y Error", yerr); SmartDashboard.putNumber("PID Y Error", yerr);
SmartDashboard.putNumber("PID Rot Error", roterr);
xoutput = xPID.update(xerr); xoutput = xPID.update(xerr);
youtput = yPID.update(yerr); youtput = yPID.update(yerr);
@@ -128,7 +129,7 @@ public class GotoLastApril extends Command {
.rotateBy(targetpos.getRotation()) .rotateBy(targetpos.getRotation())
.getCos()); .getCos());
swerveDrive.driveRelativeLockedAngle(leftStick, targetpos.getRotation()); swerveDrive.driveRelativeAngle(leftStick, targetpos.getRotation());
// swerveDrive.driveWithInputOrientation(leftStick, rightStick, true); // swerveDrive.driveWithInputOrientation(leftStick, rightStick, true);
} }
src/main/java/frc4388/robot/commands/LidarAlign.java
+1 -1
View File
@@ -41,7 +41,7 @@ public class LidarAlign extends Command {
this.currentFinderTick = 0; this.currentFinderTick = 0;
this.speed = 0.4; // TODO: find good speed for this this.speed = 0.4; // TODO: find good speed for this
this.foundReef = false; this.foundReef = false;
this.headedRight = !(GotoLastApril.tagRelativeXError < 0); this.headedRight = (GotoLastApril.tagRelativeXError < 0);
this.additionalDistance = 0; this.additionalDistance = 0;
this.bounces = 0; this.bounces = 0;
} }
src/main/java/frc4388/robot/commands/WhileTrueCommand.java
+6 -4
View File
@@ -34,24 +34,26 @@ public class WhileTrueCommand extends Command {
*/ */
@SuppressWarnings("this-escape") @SuppressWarnings("this-escape")
public WhileTrueCommand(Command whileTrue, BooleanSupplier condition) { public WhileTrueCommand(Command whileTrue, BooleanSupplier condition) {
m_whileTrue = requireNonNullParam(whileTrue, "onTrue", "WhileTrueCommand"); m_whileTrue = requireNonNullParam(whileTrue, "whileTrue", "WhileTrueCommand");
m_condition = requireNonNullParam(condition, "condition", "WhileTrueCommand"); m_condition = requireNonNullParam(condition, "condition", "WhileTrueCommand");
CommandScheduler.getInstance().registerComposedCommands(whileTrue); //CommandScheduler.getInstance().registerComposedCommands(whileTrue);
addRequirements(whileTrue.getRequirements()); // addRequirements(whileTrue.getRequirements());
} }
@Override @Override
public void initialize() { public void initialize() {
if(m_condition.getAsBoolean()) if(m_condition.getAsBoolean())
m_whileTrue.initialize(); m_whileTrue.initialize();
} }
@Override @Override
public void execute() { public void execute() {
m_whileTrue.execute(); m_whileTrue.execute();
System.out.println("Loop, " + !m_whileTrue.isFinished() + ", " + m_condition.getAsBoolean());
if(!m_whileTrue.isFinished()) if(!m_whileTrue.isFinished())
return; return;
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+17 -2
View File
@@ -141,7 +141,7 @@ public class SwerveDrive extends Subsystem {
if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05) // if no imput if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05) // if no imput
return; // don't bother doing swerve drive math and return early. return; // don't bother doing swerve drive math and return early.
leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset); leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset);
stopped = false; stopped = false;
if (fieldRelative) { if (fieldRelative) {
@@ -213,13 +213,28 @@ public class SwerveDrive extends Subsystem {
.withTargetDirection(rightStick.getAngle())); .withTargetDirection(rightStick.getAngle()));
} }
public void driveRelativeAngle(Translation2d leftStick, Rotation2d heading) {
leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset);
leftStick = TimesNegativeOne.invert(leftStick, TimesNegativeOne.XAxis, TimesNegativeOne.YAxis);
var ctrl = new SwerveRequest.FieldCentricFacingAngle()
.withVelocityX(leftStick.getX() * speedAdjust)
.withVelocityY(leftStick.getY() * speedAdjust)
.withTargetDirection(heading);
ctrl.HeadingController.setPID(
SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kP,
SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kI,
SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kD
);
swerveDriveTrain.setControl(ctrl);
}
public void driveRelativeLockedAngle(Translation2d leftStick, Rotation2d heading) { public void driveRelativeLockedAngle(Translation2d leftStick, Rotation2d heading) {
leftStick = leftStick.rotateBy(heading); leftStick = leftStick.rotateBy(heading);
var ctrl = new SwerveRequest.FieldCentricFacingAngle() var ctrl = new SwerveRequest.FieldCentricFacingAngle()
.withVelocityX(leftStick.getX() * speedAdjust) .withVelocityX(leftStick.getX() * speedAdjust)
.withVelocityY(leftStick.getY() * speedAdjust) .withVelocityY(leftStick.getY() * speedAdjust)
.withTargetDirection(Rotation2d.fromDegrees(rotTarget)); .withTargetDirection(heading);
ctrl.HeadingController.setPID( ctrl.HeadingController.setPID(
SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kP, SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kP,
SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kI, SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kI,