Skip to content

Changing alignment to be based on via OCR. #326

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

Draft
rokadias wants to merge 2 commits into
base: main
Choose a base branch
from
Draft

Changing alignment to be based on via OCR. #326

Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
5608780
Changing alignment to be based on via OCR.
rokadias Mar 26, 2025
e93afe8
Merge branch 'main' into saqib/AlignByOCR
rokadias Mar 26, 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
48 changes: 21 additions & 27 deletions src/main/java/competition/subsystems/drive/logic/AlignWithCreeperCalculator.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -115,19 +115,19 @@ public boolean initialize() {
}

if(this.leftOffsetPixelsSubscriber == null){
this.leftOffsetPixelsSubscriber = new CachedSubscriber(hostname + "." + tableLeftDistance, client,5);
this.leftOffsetPixelsSubscriber = new CachedSubscriber(hostname + "." + tableLeftDistance, client, 5);
}

if(this.rightOffsetPixelsSubscriber == null){
this.rightOffsetPixelsSubscriber = new CachedSubscriber(hostname + "." + tableRightDistance, client,5);
this.rightOffsetPixelsSubscriber = new CachedSubscriber(hostname + "." + tableRightDistance, client, 5);
}

if(this.hresSubscriber == null){
this.hresSubscriber = new CachedSubscriber(hostname + "." + tableHres, client,2);
this.hresSubscriber = new CachedSubscriber(hostname + "." + tableHres, client, 2);
}

if(this.vresSubscriber == null){
this.vresSubscriber = new CachedSubscriber(hostname + "." + tableVres, client,2);
this.vresSubscriber = new CachedSubscriber(hostname + "." + tableVres, client, 2);
}
initalized = true;
return true;
Expand All @@ -145,7 +145,6 @@ public boolean executeAlignment() {
}

if(forceStop){
// log.info("FORCE STOP");
drive.stop();
return false;
}
Expand Down Expand Up @@ -173,11 +172,17 @@ public boolean executeAlignment() {
return false;
}

// If right is not detected assume it's 1 pixel outside of the camera when determining where to move.
// This can have an error margin and think it's centered in some scenarios. We may want to change this to 2x currentCamHres.
var rightAdustedDistance = rightDistance == -1 ? this.currentCamHres + 1 : rightDistance;
var targetOffset = this.calculateOffset(leftDistance, rightAdustedDistance);
aKitLog.record("Target offset pixels from center", targetOffset);

if(leftDistance == -1 || rightDistance == -1){
this.isCenteredConfidently = false;
}
else{
double resAdjustedDiff = normalizeWidth(Math.abs(leftDistance - rightDistance));
double resAdjustedDiff = Math.abs(targetOffset);
this.isCenteredConfidently = resAdjustedDiff < this.errorThresholdPixels.get();
}

Expand All @@ -197,16 +202,9 @@ public boolean executeAlignment() {
// Determine which side the misalignment error should be taken from.
boolean offToTheLeft;

if (leftDistance == -1) {
offToTheLeft = false; // we dont see left edge, means right side is "too" visible eg off to the right
} else if (rightDistance == -1) {
offToTheLeft = true; // we dont see right edge, means left side is "too" visible eg off to the left
} else {
// Choose the pixel error value from the side indicating misalignment.
offToTheLeft = leftDistance < rightDistance; // we see both edges, so the one that is "closer to the center" is the one we are off by
}
offToTheLeft = targetOffset > 0;

aKitLog.record("Off to the left?: ",offToTheLeft);
aKitLog.record("Off to the left?: ", offToTheLeft);

// Calculate the error as a function of the left and right distance from the center.
error = costFunc(leftDistance, rightDistance);
Expand All @@ -221,7 +219,7 @@ public boolean executeAlignment() {

}

aKitLog.record("creeper error from cost function: ",error);
aKitLog.record("creeper error from cost function: ", error);

// power.
double drivePower = driveGain.get() * pidManager.calculate(0, error);
Expand All @@ -246,12 +244,10 @@ private double costFunc(int leftErrPX, int rightErrPX){
return maxError.get();
}
// abs error
double err = normalizeWidth(Math.abs(leftErrPX-rightErrPX));

// /**Linear Error**/
double errFunc = err*errorSlope.get();

double err = this.calculateOffset(leftErrPX, rightErrPX);

// Linear Error
double errFunc = err * errorSlope.get();
return Math.max(-maxError.get(), Math.min(errFunc, maxError.get())); // clip
}

Expand All @@ -276,11 +272,9 @@ public void setCamera(Cameras camera) {
this.camera = camera;
}

private double normalizeWidth(double width){
return width * this.currentCamHres / this.tunedWidth;
}

private double normalizeHeight(double height){
return height * this.currentCamVres / this.tunedHeight;
private double calculateOffset(int leftDistance, int rightDistance) {
var cameraCenterPixels = (this.currentCamHres / 2.0);
var targetCenterPixels = (leftDistance + rightDistance) / 2.0;
return targetCenterPixels - cameraCenterPixels;
}
}