Skip to content

Commit bda8cc9

Browse files
jdugan0jdugan0
authored
Merge pull request #248 from FRC1640/feature/local-align-refactor
Feature/local align refactor
2 parents 2b8cd73 + 8c2faad commit bda8cc9

12 files changed

Lines changed: 629 additions & 47 deletions

File tree

‎src/main/java/frc/robot/RobotContainer.java‎

Lines changed: 58 additions & 23 deletions
Original file line numberDiff line numberDiff line change
@@ -65,9 +65,11 @@
6565
import frc.robot.subsystems.drive.commands.DriveCommandFactory;
6666
import frc.robot.subsystems.drive.commands.DriveWeightCommand;
6767
import frc.robot.subsystems.drive.weights.AntiTipWeight;
68+
import frc.robot.subsystems.drive.weights.DynamicAlignWeight;
6869
import frc.robot.subsystems.drive.weights.FollowPathDirect;
6970
import frc.robot.subsystems.drive.weights.FollowPathNearest;
7071
import frc.robot.subsystems.drive.weights.JoystickDriveWeight;
72+
import frc.robot.subsystems.drive.weights.LocalTagAlignWeight;
7173
import frc.robot.subsystems.drive.weights.PathplannerWeight;
7274
import frc.robot.subsystems.gantry.GantryIO;
7375
import frc.robot.subsystems.gantry.GantryIOSim;
@@ -148,6 +150,8 @@ public class RobotContainer {
148150

149151
private FollowPathNearest followPathReef;
150152
private FollowPathDirect followPathCoral;
153+
private LocalTagAlignWeight localAlign;
154+
private DynamicAlignWeight dynamicAlign;
151155

152156
private final JoystickDriveWeight joystickDriveWeight;
153157

@@ -167,15 +171,15 @@ public RobotContainer() {
167171
case REAL:
168172
new CoProcessInput(new OrangePILogger());
169173
gyro = new Gyro(new GyroIONavX());
170-
aprilTagVisions.add(
171-
new AprilTagVision(
172-
new AprilTagVisionIOPhotonvision(CameraConstants.frontCameraRight),
173-
CameraConstants.frontCameraRight));
174+
// aprilTagVisions.add(
175+
// new AprilTagVision(
176+
// new AprilTagVisionIOPhotonvision(CameraConstants.frontCameraRight),
177+
// CameraConstants.frontCameraRight));
174178

175-
aprilTagVisions.add(
176-
new AprilTagVision(
177-
new AprilTagVisionIOPhotonvision(CameraConstants.frontCameraLeft),
178-
CameraConstants.frontCameraLeft));
179+
// aprilTagVisions.add(
180+
// new AprilTagVision(
181+
// new AprilTagVisionIOPhotonvision(CameraConstants.frontCameraLeft),
182+
// CameraConstants.frontCameraLeft));
179183

180184
aprilTagVisions.add(
181185
new AprilTagVision(
@@ -298,20 +302,9 @@ public RobotContainer() {
298302
algaeCommandFactory,
299303
algaeIntakeSubsystem);
300304
AprilTagVision[] visionArray = aprilTagVisions.toArray(AprilTagVision[]::new);
301-
generateNamedCommands();
302305
driveSubsystem = new DriveSubsystem(gyro);
303306
driveCommandFactory = new DriveCommandFactory(driveSubsystem);
304307
robotOdometry = new RobotOdometry(driveSubsystem, gyro, visionArray);
305-
dashboard =
306-
new Dashboard(
307-
driveSubsystem,
308-
liftSubsystem,
309-
gantrySubsystem,
310-
climberSubsystem,
311-
algaeIntakeSubsystem,
312-
coralOuttakeSubsystem,
313-
winchSubsystem,
314-
driveController);
315308
alertsManager = new AlertsManager();
316309
AlertsManager.addAlert(
317310
() -> RobotController.getBatteryVoltage() < WarningThresholdConstants.minBatteryVoltage,
@@ -383,6 +376,21 @@ public RobotContainer() {
383376
DriveWeightCommand.create(
384377
driveCommandFactory, () -> liftSubsystem.getMotorPosition() > 0.3));
385378

379+
localAlign =
380+
new LocalTagAlignWeight(
381+
() ->
382+
DistanceManager.getNearestPosition(
383+
RobotOdometry.instance.getPose("Main"),
384+
AllianceManager.chooseFromAlliance(
385+
FieldConstants.reefPositionsBlue, FieldConstants.reefPositionsRed)),
386+
() -> RobotOdometry.instance.getPose("Main").getRotation(),
387+
driveSubsystem,
388+
driveCommandFactory,
389+
gyro,
390+
visionArray);
391+
392+
dynamicAlign = new DynamicAlignWeight(followPathReef, localAlign);
393+
386394
// winchSubsystem.setDefaultCommand(
387395
// climberCommandFactory.winchApplyVoltageCommand(() -> -operatorController.getLeftY() *
388396
// 4));
@@ -399,7 +407,8 @@ public void periodic() {
399407
Logger.recordOutput("AlgaeMode", algaeMode);
400408
Logger.recordOutput("CoralPreset", coralPreset);
401409
Logger.recordOutput("TargetPosAutoalign", getTarget());
402-
Logger.recordOutput("AutoAlignDone", followPathReef.isAutoalignComplete());
410+
Logger.recordOutput("AutoAlignDone", dynamicAlign.isAutoalignComplete());
411+
Logger.recordOutput("LocalTagAlign/alignStage", dynamicAlign.getStage());
403412
Logger.recordOutput("LiftDone", liftSubsystem.isAtPreset(presetActive));
404413
Logger.recordOutput(
405414
"LiftDoneAuto", liftSubsystem.isAtPreset(coralPreset.getLift()));
@@ -420,6 +429,19 @@ public void periodic() {
420429
Logger.recordOutput("target", getTarget());
421430
}
422431
});
432+
433+
generateNamedCommands();
434+
driveSubsystem.configurePathplanner();
435+
dashboard =
436+
new Dashboard(
437+
driveSubsystem,
438+
liftSubsystem,
439+
gantrySubsystem,
440+
climberSubsystem,
441+
algaeIntakeSubsystem,
442+
coralOuttakeSubsystem,
443+
winchSubsystem,
444+
driveController);
423445
}
424446

425447
public Pose2d coralAdjust(Pose2d pose, Supplier<CoralPreset> preset) {
@@ -474,14 +496,19 @@ private void configureBindings() {
474496
.getTranslation()
475497
.getDistance(getTarget().getTranslation())
476498
< 1.3
477-
&& followPathReef.isEnabled()
499+
&& dynamicAlign.isEnabled()
478500
&& Robot.getState() != RobotState.AUTONOMOUS)
479501
.onTrue(setupAutoPlace(() -> coralPreset));
480502
// coral place routine for autoalign
481503
// new Trigger(() -> coralAutoAlignWeight.isAutoalignComplete())
482504
// .onTrue(new InstantCommand(() -> driveController.setRumble(RumbleType.kRightRumble, 1)));
483-
followPathReef.generateTrigger(
484-
() -> driveHID.getAButton() && !followPathReef.isAutoalignComplete());
505+
// followPathReef.generateTrigger(
506+
// () ->
507+
// driveController.a().getAsBoolean()
508+
// && !followPathReef.isAutoalignComplete());
509+
DriveWeightCommand.createWeightTrigger(
510+
dynamicAlign,
511+
() -> driveController.a().getAsBoolean() && !dynamicAlign.globalAlignComplete());
485512
followPathCoral.generateTrigger(
486513
() -> driveHID.getLeftBumperButton() && !followPathCoral.isAutoalignComplete());
487514

@@ -984,5 +1011,13 @@ public void generateNamedCommands() {
9841011
NamedCommands.registerCommand("PlaceTrough", autoScoringCommandFactory.placeTrough());
9851012
NamedCommands.registerCommand(
9861013
"logtest", new InstantCommand(() -> Logger.recordOutput("logtest", true)));
1014+
1015+
NamedCommands.registerCommand(
1016+
"LocalAlign",
1017+
localAlign
1018+
.getAutoCommand()
1019+
.deadlineFor(autonAutoPlace(() -> coralPreset))
1020+
.until(() -> localAlign.isAutoalignComplete() || !localAlign.isReady()));
1021+
NamedCommands.registerCommand("WaitForLocal", new WaitUntilCommand(() -> localAlign.isReady()));
9871022
}
9881023
}

‎src/main/java/frc/robot/constants/FieldConstants.java‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -19,6 +19,7 @@ public class FieldConstants {
1919
AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeAndyMark))
2020
.addValue(AprilTagSetting.WPICal, WPICALPosManager.aprilTagFieldLayout)
2121
.get();
22+
2223
public static final int tagCount = 22;
2324

2425
public static final VisionSystemSim visionSim = new VisionSystemSim("main");

‎src/main/java/frc/robot/constants/RobotConstants.java‎

Lines changed: 39 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -11,6 +11,7 @@
1111
import edu.wpi.first.math.numbers.N1;
1212
import edu.wpi.first.math.numbers.N3;
1313
import edu.wpi.first.math.trajectory.TrapezoidProfile;
14+
import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints;
1415
import edu.wpi.first.math.util.Units;
1516
import frc.robot.sensors.resolvers.ResolverVoltageInfo;
1617
import frc.robot.subsystems.drive.DriveSubsystem;
@@ -29,6 +30,7 @@ public class RobotConstants {
2930
public class RobotDimensions {
3031
public static final double robotWidth = 0.81;
3132
public static final double robotLength = 0.81; // 0.927
33+
public static final double robotLengthLocalAlign = 0.79 + 0.16;
3234
public static final Translation2d robotXY = new Translation2d(robotWidth / 2, robotLength / 2);
3335
}
3436

@@ -111,11 +113,16 @@ public static enum PivotId {
111113
public static class AutoAlignConfig {
112114
public static final double maxDistanceFromTarget = 0.3;
113115
public static final PathConstraints coralStationPathConstraints =
114-
new PathConstraints(2.5, 2.5, Math.PI + 0.75, 4 * Math.PI);
116+
new PathConstraints(3, 3.5, Math.PI + 0.75, 4 * Math.PI);
115117
public static final PathConstraints coralStationPathConstraintsSlow =
116-
new PathConstraints(1.3, 2.5, Math.PI + 0.75, 4 * Math.PI);
118+
new PathConstraints(2, 3, Math.PI + 0.75, 4 * Math.PI);
117119
public static final PathConstraints pathConstraints =
118-
new PathConstraints(2, 2, Math.PI, 4 * Math.PI);
120+
new PathConstraints(3, 3.5, Math.PI, 4 * Math.PI);
121+
122+
// local align
123+
public static final Constraints localAlignPpidConstraints =
124+
new Constraints(DriveConstants.maxSpeed, 3);
125+
public static final double profiledDistThreshold = 0.4;
119126
}
120127

121128
public static class DriveConstants {
@@ -165,6 +172,19 @@ public static class DriveConstants {
165172
}
166173

167174
public static class CameraConstants {
175+
// public static final CameraConstant frontCameraLeft =
176+
// new CameraConstant(
177+
// new SimCameraProperties(),
178+
// new Transform3d(
179+
// new Translation3d(
180+
// Units.inchesToMeters(13.95),
181+
// Units.inchesToMeters(11.9),
182+
// Units.inchesToMeters(12.125)),
183+
// new Rotation3d(0, Math.toRadians(10.5), -Math.toRadians(15))),
184+
// 1,
185+
// "Sommar",
186+
// "Front Left");
187+
168188
public static final CameraConstant frontCameraLeft =
169189
new CameraConstant(
170190
new SimCameraProperties(),
@@ -175,9 +195,22 @@ public static class CameraConstants {
175195
Units.inchesToMeters(12.125)),
176196
new Rotation3d(0, Math.toRadians(10.5), -Math.toRadians(15))),
177197
1,
178-
"Sommar",
198+
"Park",
179199
"Front Left");
180200

201+
// public static final CameraConstant frontCameraRight =
202+
// new CameraConstant(
203+
// new SimCameraProperties(),
204+
// new Transform3d(
205+
// new Translation3d(
206+
// Units.inchesToMeters(13.95),
207+
// -Units.inchesToMeters(11.9),
208+
// Units.inchesToMeters(12.125)),
209+
// new Rotation3d(0, Math.toRadians(10.5), Math.toRadians(15))),
210+
// 1.1,
211+
// "Dodds",
212+
// "Front Right");
213+
181214
public static final CameraConstant frontCameraRight =
182215
new CameraConstant(
183216
new SimCameraProperties(),
@@ -188,7 +221,7 @@ public static class CameraConstants {
188221
Units.inchesToMeters(12.125)),
189222
new Rotation3d(0, Math.toRadians(10.5), Math.toRadians(15))),
190223
1.1,
191-
"Dodds",
224+
"Arducam_OV2311_USB_Camera",
192225
"Front Right");
193226

194227
public static final CameraConstant frontCameraCenter =
@@ -370,7 +403,7 @@ public static class GantryConstants {
370403
Units.inchesToMeters(0.5) * 1.13278894472 * 0.60103626943 * 1.58904109589 * 1.03571428571;
371404
// left -> right limit
372405
public static final Limits gantryLimits = new Limits(0.01, 0.36 + Units.inchesToMeters(1));
373-
public static final double gantryLimitCenter = 0.198;
406+
public static final double gantryLimitCenter = 0.223; // 0.198
374407
public static final double gantryPadding = 0.02;
375408
public static final int gantryLimitSwitchDIOPort = new RobotSwitch<Integer>(4).get();
376409
public static final double alignSpeed = 0.2;

‎src/main/java/frc/robot/constants/RobotPIDConstants.java‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -124,4 +124,10 @@ public static final MAXMotionConfig constructMaxMotionPos(PIDConstants constant)
124124
public static final PIDConstants climberWinchPIDFast = new PIDConstants(0.75, 0, 0);
125125

126126
public static final PIDConstants driveAntiTip = new PIDConstants(1, 0, 0);
127+
128+
public static final PIDConstants localTagAlign = new PIDConstants(0.8, 0.000, 0.0005);
129+
public static final PIDConstants localTagAlignVelocity = new PIDConstants(0.25, 0, 0);
130+
// public static final PIDConstants localTagAlignY = new PIDConstants(0.25, 0, 0);
131+
public static final PIDConstants localAnglePid = new PIDConstants(0.5, 0.001, 0.001);
132+
public static final PIDConstants localDriveProfiledPid = new PIDConstants(0.5, 0, 0);
127133
}

0 commit comments

Comments
 (0)