Skip to content

Separate lineup offsets in constants #254

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

Merged
aidnem merged 6 commits into from
Oct 24, 2025
Merged

Separate lineup offsets in constants #254

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
5026bf8
(#252) Untested - Add separate along track offsets for each camera an…
aidnem Oct 10, 2025
701d91a
(#252) Untested - Implement usage of lineup heading offsets
aidnem Oct 11, 2025
3e5c402
(#252) Untested - Update left lineup gains based on shop testing
frcteam401 Oct 13, 2025
69fe666
(#252) Update other side lineup constants and fail to fix climb
frcteam401 Oct 22, 2025
13ec3b6
(#252) Use camera indices from JSON Constants rather than hardcoding …
aidnem Oct 24, 2025
2374960
(#252) Update non-comp JSON environments with new separate alongtrack…
aidnem Oct 24, 2025
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
2 changes: 1 addition & 1 deletion src/main/deploy/constants/comp/ClimbConstants.json
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -11,7 +11,7 @@
"value": 0.6,
"unit": "Radian"
},
"climbEncoderOffsetRotations": -0.40234375,
"climbEncoderOffsetRotations": -0.238037109375,
Comment thread
godmar marked this conversation as resolved.
"leadClimbMotorId": 16,
"followerClimbMotorId": 17,
"climbkP": 45.0,
Expand Down
13 changes: 11 additions & 2 deletions src/main/deploy/constants/comp/DrivetrainConstants.json
View file
Open in desktop
Comment thread
godmar marked this conversation as resolved.
Original file line number Diff line number Diff line change
Expand Up @@ -38,11 +38,20 @@
"driveRotationKi": 0.0,
"driveRotationKd": 0.0,
"driveAlongTrackMultiplier": 1,
"driveAlongTrackOffset": 0.24,
"driveCrossTrackFrontRightOffset": -0.03,
"driveAlongTrackFrontRightOffset": 0.25,
"driveAlongTrackFrontLeftOffset": 0.24,
"driveCrossTrackFrontRightOffset": -0.02,
"driveCrossTrackFrontLeftOffset": -0.03,
"driveCrossTrackFrontRightAlgaeOffset": 0.25,
"driveCrossTrackFrontLeftAlgaeOffset": 0.25,
"driveLineupHeadingFrontRightOffset": {
"value": 0.0,
"unit": "Degree"
},
"driveLineupHeadingFrontLeftOffset": {
"value": 0.0,
"unit": "Degree"
},
"lineupRotationMarginRadians": 0.05,
"lineupAlongTrackThresholdMeters": 0.05,
"lineupCrossTrackThresholdMeters": 0.04,
Expand Down
11 changes: 10 additions & 1 deletion src/main/deploy/constants/demo/DrivetrainConstants.json
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -38,11 +38,20 @@
"driveRotationKi": 0.0,
"driveRotationKd": 0.0,
"driveAlongTrackMultiplier": 1,
"driveAlongTrackOffset": 0.27,
"driveAlongTrackFrontRightOffset": 0.25,
"driveAlongTrackFrontLeftOffset": 0.24,
"driveCrossTrackFrontRightOffset": -0.02,
"driveCrossTrackFrontLeftOffset": -0.03,
"driveCrossTrackFrontRightAlgaeOffset": 0.25,
"driveCrossTrackFrontLeftAlgaeOffset": 0.25,
"driveLineupHeadingFrontRightOffset": {
"value": 0.0,
"unit": "Degree"
},
"driveLineupHeadingFrontLeftOffset": {
"value": 0.0,
"unit": "Degree"
},
"lineupRotationMarginRadians": 0.05,
"lineupAlongTrackThresholdMeters": 0.05,
"lineupCrossTrackThresholdMeters": 0.04,
Expand Down
11 changes: 10 additions & 1 deletion src/main/deploy/constants/hitl/DrivetrainConstants.json
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -28,8 +28,17 @@
"driveRotationKi": 0.0,
"driveRotationKd": 0.0,
"driveAlongTrackMultiplier": 1,
"driveAlongTrackOffset": 0.25,
"driveAlongTrackFrontRightOffset": 0.25,
"driveAlongTrackFrontLeftOffset": 0.25,
"driveCrossTrackFrontRightOffset": 0.0,
"driveLineupHeadingFrontRightOffset": {
"value": 0.0,
"unit": "Degree"
},
"driveLineupHeadingFrontLeftOffset": {
"value": 0.0,
"unit": "Degree"
},
"otfPoseDistanceLimit": 0.1,
"lineupMaxVelocity": 3.0,
"lineupMaxAcceleration": 1.5,
Expand Down
11 changes: 10 additions & 1 deletion src/main/deploy/constants/test_drivebase/DrivetrainConstants.json
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -28,8 +28,17 @@
"driveRotationKi": 0.0,
"driveRotationKd": 0.0,
"driveAlongTrackMultiplier": 1,
"driveAlongTrackOffset": 0.25,
"driveAlongTrackFrontRightOffset": 0.25,
"driveAlongTrackFrontLeftOffset": 0.25,
"driveCrossTrackFrontRightOffset": 0.0,
"driveLineupHeadingFrontRightOffset": {
"value": 0.0,
"unit": "Degree"
},
"driveLineupHeadingFrontLeftOffset": {
"value": 0.0,
"unit": "Degree"
},
"otfPoseDistanceLimit": 0.1,
"lineupMaxVelocity": 3.0,
"lineupMaxAcceleration": 1.5,
Expand Down
12 changes: 6 additions & 6 deletions src/main/java/frc/robot/BuildConstants.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5,12 +5,12 @@ public final class BuildConstants {
public static final String MAVEN_GROUP = "";
public static final String MAVEN_NAME = "2025-Robot-Code";
public static final String VERSION = "unspecified";
public static final int GIT_REVISION = 94;
public static final String GIT_SHA = "8f0396d74d2a8c697941aada0550766844303745";
public static final String GIT_DATE = "2025-08-22 17:46:12 EDT";
public static final String GIT_BRANCH = "248-add-readme";
public static final String BUILD_DATE = "2025-08-23 21:35:05 EDT";
public static final long BUILD_UNIX_TIME = 1755999305676L;
public static final int GIT_REVISION = 90;
public static final String GIT_SHA = "13ec3b6f046838dd98b05d9f82f2362eb77383c4";
public static final String GIT_DATE = "2025-10-24 15:19:25 EDT";
public static final String GIT_BRANCH = "252-separate-lineup-offsets";
public static final String BUILD_DATE = "2025-10-24 15:21:57 EDT";
public static final long BUILD_UNIX_TIME = 1761333717458L;
public static final int DIRTY = 1;

private BuildConstants() {}
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/constants/VisionConstants.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,7 @@ public class VisionConstants {
EnvironmentHandler.getEnvironmentHandler().getEnvironmentPathProvider(),
new JSONSyncConfigBuilder().setPrettyPrinting(true).build());

public final Integer FrontLeftCameraIndex = 0;
public final Integer FrontLeftCameraIndex = 1;
public final Integer FrontRightCameraIndex = 0;
public final Transform3d FrontLeftTransform = new Transform3d();
public final Transform3d FrontRightTransform =
Expand Down
5 changes: 4 additions & 1 deletion src/main/java/frc/robot/constants/subsystems/DrivetrainConstants.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -84,11 +84,14 @@ public class DrivetrainConstants {
public final Double driveRotationKi = 0.0;
public final Double driveRotationKd = 0.0;
public final Double driveAlongTrackMultiplier = -1.0;
public final Double driveAlongTrackOffset = 0.25;
public final Double driveAlongTrackFrontRightOffset = 0.25;
public final Double driveAlongTrackFrontLeftOffset = 0.25;
public final Double driveCrossTrackFrontRightOffset = 0.0;
public final Double driveCrossTrackFrontLeftOffset = 0.0;
public final Double driveCrossTrackFrontRightAlgaeOffset = 0.25;
public final Double driveCrossTrackFrontLeftAlgaeOffset = 0.25;
public final Angle driveLineupHeadingFrontRightOffset = Degrees.zero();
public final Angle driveLineupHeadingFrontLeftOffset = Degrees.zero();

public final Double lineupMaxVelocity = 3.0;
public final Double lineupMaxAcceleration = 1.5;
Expand Down
36 changes: 30 additions & 6 deletions src/main/java/frc/robot/subsystems/drive/states/LineupState.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,5 +1,7 @@
package frc.robot.subsystems.drive.states;

import static edu.wpi.first.units.Units.Radians;

import coppercore.controls.state_machine.state.PeriodicStateInterface;
import coppercore.controls.state_machine.transition.Transition;
import coppercore.parameter_tools.LoggedTunableNumber;
Expand All @@ -11,6 +13,7 @@
import edu.wpi.first.math.geometry.Transform2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.units.measure.Angle;
import edu.wpi.first.wpilibj.DriverStation;
import frc.robot.StrategyManager;
import frc.robot.StrategyManager.AutonomyMode;
Expand Down Expand Up @@ -337,13 +340,21 @@ public DistanceToTag tryOtherCamera(
? 0
: JsonConstants.visionConstants.FrontLeftTransform.getY()
- JsonConstants.visionConstants.FrontRightTransform.getY();

double alongTrackOffset;
if (otherCameraIndex == JsonConstants.visionConstants.FrontLeftCameraIndex) {
alongTrackOffset = JsonConstants.drivetrainConstants.driveAlongTrackFrontLeftOffset;
} else {
alongTrackOffset = JsonConstants.drivetrainConstants.driveAlongTrackFrontRightOffset;
}

DistanceToTag observationOtherCamera =
alignmentSupplier.get(
tagId,
otherCameraIndex,
ReefLineupUtil.getCrossTrackOffset(otherCameraIndex)
+ (signOfError * offsetErrorCorrection),
JsonConstants.drivetrainConstants.driveAlongTrackOffset);
alongTrackOffset);
return observationOtherCamera;
}

Expand Down Expand Up @@ -392,12 +403,16 @@ public void LineupWithReefLocation() {
return;
}

double alongTrackOffset;
if (cameraIndex == JsonConstants.visionConstants.FrontLeftCameraIndex) {
alongTrackOffset = JsonConstants.drivetrainConstants.driveAlongTrackFrontLeftOffset;
} else {
alongTrackOffset = JsonConstants.drivetrainConstants.driveAlongTrackFrontRightOffset;
}

DistanceToTag observation =
alignmentSupplier.get(
tagId,
cameraIndex,
ReefLineupUtil.getCrossTrackOffset(cameraIndex),
JsonConstants.drivetrainConstants.driveAlongTrackOffset);
tagId, cameraIndex, ReefLineupUtil.getCrossTrackOffset(cameraIndex), alongTrackOffset);

DistanceToTag otherCameraObs = tryOtherCamera(alignmentSupplier, tagId, cameraIndex);

Expand Down Expand Up @@ -460,9 +475,18 @@ public void LineupWithReefLocation() {
* getAlongTrackVelocityReductionFactor(observation.crossTrackDistance())
* getAlongTrackVelocity(observation.alongTrackDistance()); // Maybe use filtered?
double vy = driveCrossTrackLineupController.calculate(observation.crossTrackDistance());

Angle headingOffset;
if (cameraIndex == JsonConstants.visionConstants.FrontLeftCameraIndex) {
headingOffset = JsonConstants.drivetrainConstants.driveLineupHeadingFrontLeftOffset;
} else {
headingOffset = JsonConstants.drivetrainConstants.driveLineupHeadingFrontRightOffset;
}

double omega =
rotationController.calculate(
drive.getRotation().getRadians(), this.getRotationForReefSide().getRadians());
drive.getRotation().getRadians(),
this.getRotationForReefSide().getRadians() + headingOffset.in(Radians));

if (usingOtherCamera) {
vy = driveCrossTrackOtherCameraLineupController.calculate(observation.crossTrackDistance());
Expand Down
Loading