Team4388/2025RidgeScape
1
0
Fork 0
mirror of https://github.com/Team4388/2025RidgeScape.git synced 2026-06-09 00:38:02 -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"
}
},
{
"type": "named",
"data": {
"name": "align-feed"
}
},
{
"type": "named",
"data": {
@@ -58,12 +52,6 @@
"pathName": "Reef 2 to Bottom Feed"
}
},
{
"type": "named",
"data": {
"name": "align-feed"
}
},
{
"type": "named",
"data": {
src/main/deploy/pathplanner/paths/Bottom Feed to Reef 2.path
+4 -4
View File
@@ -3,13 +3,13 @@
"waypoints": [
{
"anchor": {
"x": 1.0603125,
"y": 1.0390625
"x": 1.0807449494949497,
"y": 1.0875631313131315
},
"prevControl": null,
"nextControl": {
"x": 2.3463856916648673,
"y": 1.797152889765366
"x": 2.3668181411598166,
"y": 1.8456535210784974
},
"isLocked": false,
"linkedName": null
src/main/deploy/pathplanner/paths/Cage 4 to Reef.path
+7 -7
View File
@@ -3,13 +3,13 @@
"waypoints": [
{
"anchor": {
"x": 7.595959595959596,
"y": 3.0289141414141416
"x": 7.006439393939393,
"y": 3.1508838383838382
},
"prevControl": null,
"nextControl": {
"x": 6.2521875,
"y": 2.6234375000000005
"x": 6.498232323232323,
"y": 2.480050505050505
},
"isLocked": false,
"linkedName": null
@@ -20,8 +20,8 @@
"y": 2.6934974747474745
},
"prevControl": {
"x": 5.630625,
"y": 2.148125
"x": 5.979861111111112,
"y": 1.992171717171717
},
"nextControl": null,
"isLocked": false,
@@ -48,7 +48,7 @@
"folder": "Barge to Reef",
"idealStartingState": {
"velocity": 0,
"rotation": -178.9832812445108
"rotation": 124.11447294534119
},
"useDefaultConstraints": true
}
src/main/deploy/pathplanner/paths/Cage 5 to Reef.path
+10 -10
View File
@@ -3,25 +3,25 @@
"waypoints": [
{
"anchor": {
"x": 7.129687499999999,
"y": 1.9409375
"x": 7.016603535353535,
"y": 1.9718434343434341
},
"prevControl": null,
"nextControl": {
"x": 6.072698863636364,
"y": 1.0434943181818186
"x": 6.000189393939394,
"y": 1.6364267676767676
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 5.2771875,
"y": 2.61125
"x": 5.47165404040404,
"y": 2.1141414141414145
},
"prevControl": {
"x": 5.504318181818181,
"y": 2.1902272727272725
"x": 5.86940340909091,
"y": 1.7593308080808083
},
"nextControl": null,
"isLocked": false,
@@ -42,13 +42,13 @@
},
"goalEndState": {
"velocity": 0,
"rotation": 118.99790479482637
"rotation": 124.8244891569568
},
"reversed": false,
"folder": "Barge to Reef",
"idealStartingState": {
"velocity": 0,
"rotation": 179.5295681977034
"rotation": 126.34745820888533
},
"useDefaultConstraints": true
}
src/main/deploy/pathplanner/paths/Cage 6 to Reef.path
+7 -7
View File
@@ -3,13 +3,13 @@
"waypoints": [
{
"anchor": {
"x": 7.565467171717171,
"y": 0.8334595959595947
"x": 7.057260101010102,
"y": 0.8537878787878788
},
"prevControl": null,
"nextControl": {
"x": 6.625889381464077,
"y": 1.4477106264692527
"x": 6.315277777777778,
"y": 1.5144570707070704
},
"isLocked": false,
"linkedName": null
@@ -20,8 +20,8 @@
"y": 2.6234375
},
"prevControl": {
"x": 5.721332938841585,
"y": 2.265481302925542
"x": 5.624116161616161,
"y": 2.205618686868687
},
"nextControl": null,
"isLocked": false,
@@ -48,7 +48,7 @@
"folder": "Barge to Reef",
"idealStartingState": {
"velocity": 0,
"rotation": -179.94258861633824
"rotation": 132.5633517531898
},
"useDefaultConstraints": true
}
src/main/deploy/pathplanner/paths/Reef 2 to Bottom Feed.path
+11 -11
View File
@@ -3,25 +3,25 @@
"waypoints": [
{
"anchor": {
"x": 3.680625,
"y": 2.61125
"x": 3.749318181818182,
"y": 2.868295454545454
},
"prevControl": null,
"nextControl": {
"x": 3.2541591082344743,
"y": 2.3264921263556886
"x": 3.093244949494949,
"y": 2.2869318181818175
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 1.12125,
"y": 1.0025
"x": 1.3246843434343436,
"y": 1.5347853535353522
},
"prevControl": {
"x": 1.3989083079519464,
"y": 1.1682576997602765
"x": 1.802550505050505,
"y": 2.1992045454545446
},
"nextControl": null,
"isLocked": false,
@@ -33,8 +33,8 @@
"pointTowardsZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 3.0,
"maxAcceleration": 3.0,
"maxVelocity": 5.0,
"maxAcceleration": 5.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0,
"nominalVoltage": 12.0,
@@ -50,5 +50,5 @@
"velocity": 0,
"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": [
{
"anchor": {
"x": 5.265,
"y": 2.61125
"x": 5.268371212121212,
"y": 2.4597222222222217
},
"prevControl": null,
"nextControl": {
"x": 4.137208815063533,
"y": 2.0700494801809173
"x": 4.670075757575758,
"y": 1.2531597222222217
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 1.096875,
"y": 1.0390625
"x": 1.1010732323232322,
"y": 1.00625
},
"prevControl": {
"x": 1.3299317623663032,
"y": 1.1501468126694903
"x": 1.786268939393939,
"y": 2.2439551767676766
},
"nextControl": null,
"isLocked": false,
@@ -33,8 +33,8 @@
"pointTowardsZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 3.0,
"maxAcceleration": 3.0,
"maxVelocity": 5.0,
"maxAcceleration": 5.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0,
"nominalVoltage": 12.0,
@@ -50,5 +50,5 @@
"velocity": 0,
"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
},
"prevControl": {
"x": 1.95,
"x": 1.9500000000000002,
"y": 2.1115625000000002
},
"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);
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 {
@@ -294,7 +294,7 @@ public final class Constants {
}
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 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 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 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 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;
public Command LoopAprilLidarAlignL4Left = new WhileTrueCommand(AprilLidarAlignL4Left, () -> !m_robotElevator.hasCoral());
public Command LoopAprilLidarAlignL4Right = new WhileTrueCommand(AprilLidarAlignL4Right, () -> !m_robotElevator.hasCoral());
public Command LoopAprilLidarAlignL3Left = new WhileTrueCommand(AprilLidarAlignL3Left, () -> !m_robotElevator.hasCoral());
public Command LoopAprilLidarAlignL3Right = new WhileTrueCommand(AprilLidarAlignL3Right, () -> !m_robotElevator.hasCoral());
public Command LoopAprilLidarAlignL2Left = new WhileTrueCommand(AprilLidarAlignL2Left, () -> !m_robotElevator.hasCoral());
public Command LoopAprilLidarAlignL2Right = new WhileTrueCommand(AprilLidarAlignL2Right, () -> !m_robotElevator.hasCoral());
// public Command LoopAprilLidarAlignL4Left = new WhileTrueCommand(AprilLidarAlignL4Left.asProxy(), () -> !m_robotElevator.hasCoral());
// public Command LoopAprilLidarAlignL4Right = new WhileTrueCommand(AprilLidarAlignL4Right.asProxy(), () -> !m_robotElevator.hasCoral());
// public Command LoopAprilLidarAlignL3Left = new WhileTrueCommand(AprilLidarAlignL3Left.asProxy(), () -> !m_robotElevator.hasCoral());
// public Command LoopAprilLidarAlignL3Right = new WhileTrueCommand(AprilLidarAlignL3Right.asProxy(), () -> !m_robotElevator.hasCoral());
// public Command LoopAprilLidarAlignL2Left = new WhileTrueCommand(AprilLidarAlignL2Left.asProxy(), () -> !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.
*/
@@ -334,15 +341,15 @@ public class RobotContainer {
NamedCommands.registerCommand("align-feed", new SequentialCommandGroup(
new MoveForTimeCommand(m_robotSwerveDrive,
new Translation2d(0, 1),
new Translation2d(), 400, true
new Translation2d(), 300, true
), new InstantCommand(()-> {m_robotSwerveDrive.softStop();} , m_robotSwerveDrive)));
NamedCommands.registerCommand("place-coral-left-l4", LoopAprilLidarAlignL4Left);
NamedCommands.registerCommand("place-coral-right-l4", LoopAprilLidarAlignL4Right);
NamedCommands.registerCommand("place-coral-left-l3", LoopAprilLidarAlignL3Left);
NamedCommands.registerCommand("place-coral-right-l3", LoopAprilLidarAlignL3Right);
NamedCommands.registerCommand("place-coral-left-l2", LoopAprilLidarAlignL2Left);
NamedCommands.registerCommand("place-coral-right-l2", LoopAprilLidarAlignL2Right);
NamedCommands.registerCommand("place-coral-left-l4", AprilLidarAlignL4Left);
NamedCommands.registerCommand("place-coral-right-l4", AprilLidarAlignL4Right);
NamedCommands.registerCommand("place-coral-left-l3", AprilLidarAlignL3Left);
NamedCommands.registerCommand("place-coral-right-l3", AprilLidarAlignL3Right);
NamedCommands.registerCommand("place-coral-left-l2", AprilLidarAlignL2Left);
NamedCommands.registerCommand("place-coral-right-l2", AprilLidarAlignL2Right);
NamedCommands.registerCommand("prepare-l4", new InstantCommand(() -> {
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;
}
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("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);
SmartDashboard.putNumber("PID X Error", xerr);
SmartDashboard.putNumber("PID Y Error", yerr);
SmartDashboard.putNumber("PID Rot Error", roterr);
xoutput = xPID.update(xerr);
youtput = yPID.update(yerr);
@@ -128,7 +129,7 @@ public class GotoLastApril extends Command {
.rotateBy(targetpos.getRotation())
.getCos());
swerveDrive.driveRelativeLockedAngle(leftStick, targetpos.getRotation());
swerveDrive.driveRelativeAngle(leftStick, targetpos.getRotation());
// 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.speed = 0.4; // TODO: find good speed for this
this.foundReef = false;
this.headedRight = !(GotoLastApril.tagRelativeXError < 0);
this.headedRight = (GotoLastApril.tagRelativeXError < 0);
this.additionalDistance = 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")
public WhileTrueCommand(Command whileTrue, BooleanSupplier condition) {
m_whileTrue = requireNonNullParam(whileTrue, "onTrue", "WhileTrueCommand");
m_whileTrue = requireNonNullParam(whileTrue, "whileTrue", "WhileTrueCommand");
m_condition = requireNonNullParam(condition, "condition", "WhileTrueCommand");
CommandScheduler.getInstance().registerComposedCommands(whileTrue);
//CommandScheduler.getInstance().registerComposedCommands(whileTrue);
addRequirements(whileTrue.getRequirements());
// addRequirements(whileTrue.getRequirements());
}
@Override
public void initialize() {
if(m_condition.getAsBoolean())
m_whileTrue.initialize();
m_whileTrue.initialize();
}
@Override
public void execute() {
m_whileTrue.execute();
System.out.println("Loop, " + !m_whileTrue.isFinished() + ", " + m_condition.getAsBoolean());
if(!m_whileTrue.isFinished())
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
return; // don't bother doing swerve drive math and return early.
leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset);
leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset);
stopped = false;
if (fieldRelative) {
@@ -213,13 +213,28 @@ public class SwerveDrive extends Subsystem {
.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) {
leftStick = leftStick.rotateBy(heading);
var ctrl = new SwerveRequest.FieldCentricFacingAngle()
.withVelocityX(leftStick.getX() * speedAdjust)
.withVelocityY(leftStick.getY() * speedAdjust)
.withTargetDirection(Rotation2d.fromDegrees(rotTarget));
.withTargetDirection(heading);
ctrl.HeadingController.setPID(
SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kP,
SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kI,