eaglesfe
/
robotcode
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Working auto

This commit is contained in:
Scott Barnes 2023-11-18 15:17:17 -06:00
parent c35ed27a09
commit 1aa49e979d
3 changed files with 19 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
TeamCode/src/main/java/opmodes/AutoBase.java
View File

@ -6,6 +6,7 @@ import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.SCORING_DISTAN
import static org.firstinspires.ftc.teamcode.util.CenterStageCommon.PropLocation.Center;
import static org.firstinspires.ftc.teamcode.util.CenterStageCommon.PropLocation.Left;
import static org.firstinspires.ftc.teamcode.util.CenterStageCommon.PropLocation.Right;
import static org.firstinspires.ftc.teamcode.util.CenterStageCommon.PropLocation.Unknown;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
@ -167,7 +168,7 @@ public abstract class AutoBase extends LinearOpMode {
private void dislodgePropAndPlacePixelCenter() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(rendezvous);
builder.lineToConstantHeading(rendezvous.vec());
builder.addDisplacementMarker(10, () -> {
this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN);
});
@ -178,7 +179,7 @@ public abstract class AutoBase extends LinearOpMode {
}
private void determinePropLocation() {
setPropLocationIfVisible(Center, null);
setPropLocationIfVisible(Center, Unknown);
if (this.propLocation != Center) {
peekRight();
}
@ -193,20 +194,6 @@ public abstract class AutoBase extends LinearOpMode {
setPropLocationIfVisible(Right, Left);
}
protected static int getExpectedAprilTagId(CenterStageCommon.PropLocation propLocation) {
switch (propLocation) {
case Left:
return 1;
case Unknown:
case Center:
return 2;
case Right:
return 3;
}
return 2;
}
protected void setPropLocationIfVisible(CenterStageCommon.PropLocation ifVisible, CenterStageCommon.PropLocation ifNotVisible) {
float seenCount = 0;
float samples = 10;

17
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Camera.java
View File

@ -18,7 +18,8 @@ import org.opencv.core.Point;
@Config
public class Camera {
public static float PROP_REJECTION_VERTICAL = 300;
public static float PROP_REJECTION_VERTICAL_UPPER = 175;
public static float PROP_REJECTION_VERTICAL_LOWER = 300;
private PropDetectionPipeline prop;
private AprilTagProcessor aprilTag;
private VisionPortal visionPortal;
@ -49,23 +50,13 @@ public class Camera {
return INVALID_DETECTION;
}
Detection detection = null;
switch (getAlliance()) {
case Blue:
Detection blue = this.prop.getBlue();
detection = blue != null && blue.isValid() ? blue : INVALID_DETECTION;
break;
return blue != null && blue.isValid() ? blue : INVALID_DETECTION;
case Red:
Detection red = this.prop.getRed();
detection = red != null && red.isValid() ? red : INVALID_DETECTION;
break;
}
if (detection != null && detection.isValid()) {
Point center = detection.getCenterPx();
if (center.y < PROP_REJECTION_VERTICAL) {
return detection;
}
return red != null && red.isValid() ? red : INVALID_DETECTION;
}
return INVALID_DETECTION;

19
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/PropDetectionPipeline.java
View File

@ -1,17 +1,16 @@
package org.firstinspires.ftc.teamcode.vision;
import static org.firstinspires.ftc.teamcode.hardware.Camera.PROP_REJECTION_VERTICAL;
import static org.firstinspires.ftc.teamcode.hardware.Camera.PROP_REJECTION_VERTICAL_LOWER;
import static org.firstinspires.ftc.teamcode.hardware.Camera.PROP_REJECTION_VERTICAL_UPPER;
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.DETECTION_AREA_MAX;
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.DETECTION_AREA_MIN;
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.DETECTION_CENTER_X;
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.DETECTION_LEFT_X;
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.DETECTION_RIGHT_X;
import static org.firstinspires.ftc.teamcode.util.Colors.FTC_BLUE_RANGE;
import static org.firstinspires.ftc.teamcode.util.Colors.FTC_RED_RANGE_1;
import static org.firstinspires.ftc.teamcode.util.Colors.FTC_RED_RANGE_2;
import static org.firstinspires.ftc.teamcode.util.Colors.RED;
import static org.firstinspires.ftc.teamcode.util.Colors.WHITE;
import static org.firstinspires.ftc.teamcode.util.Constants.ANCHOR;
import static org.firstinspires.ftc.teamcode.util.Constants.BLACK;
import static org.firstinspires.ftc.teamcode.util.Constants.BLUR_SIZE;
import static org.firstinspires.ftc.teamcode.util.Constants.ERODE_DILATE_ITERATIONS;
import static org.firstinspires.ftc.teamcode.util.Constants.STRUCTURING_ELEMENT;
@ -91,6 +90,9 @@ public class PropDetectionPipeline implements VisionProcessor {
Core.add(mask, tmpMask, mask);
}
Imgproc.rectangle(mask, new Point(0,0), new Point(mask.cols() - 1, (int)PROP_REJECTION_VERTICAL_UPPER), BLACK, -1);
Imgproc.rectangle(mask, new Point(0,(int)PROP_REJECTION_VERTICAL_LOWER), new Point(mask.cols() - 1, mask.rows() -1), BLACK, -1);
Imgproc.erode(mask, mask, STRUCTURING_ELEMENT, ANCHOR, ERODE_DILATE_ITERATIONS);
Imgproc.dilate(mask, mask, STRUCTURING_ELEMENT, ANCHOR, ERODE_DILATE_ITERATIONS);
@ -102,11 +104,13 @@ public class PropDetectionPipeline implements VisionProcessor {
@Override
public void onDrawFrame(Canvas canvas, int onscreenWidth, int onscreenHeight, float scaleBmpPxToCanvasPx, float scaleCanvasDensity, Object userContext) {
canvas.drawLine(0, PROP_REJECTION_VERTICAL, canvas.getWidth(), PROP_REJECTION_VERTICAL, WHITE);
canvas.drawLine(0, PROP_REJECTION_VERTICAL_LOWER, canvas.getWidth(), PROP_REJECTION_VERTICAL_LOWER, WHITE);
canvas.drawLine(0, PROP_REJECTION_VERTICAL_UPPER, canvas.getWidth(), PROP_REJECTION_VERTICAL_UPPER, WHITE);
if (red != null && red.isValid()) {
Point center = red.getCenterPx();
if (center.y < PROP_REJECTION_VERTICAL) {
if (center.y < PROP_REJECTION_VERTICAL_LOWER
&& center.y > PROP_REJECTION_VERTICAL_UPPER) {
canvas.drawCircle((float)red.getCenterPx().x, (float)red.getCenterPx().y, 10, WHITE);
} else {
canvas.drawCircle((float)red.getCenterPx().x, (float)red.getCenterPx().y, 10, RED);
@ -115,7 +119,8 @@ public class PropDetectionPipeline implements VisionProcessor {
if (blue != null && blue.isValid()) {
Point center = blue.getCenterPx();
if (center.y < PROP_REJECTION_VERTICAL) {
if (center.y < PROP_REJECTION_VERTICAL_LOWER
&& center.y > PROP_REJECTION_VERTICAL_UPPER) {
canvas.drawCircle((float)blue.getCenterPx().x, (float)blue.getCenterPx().y, 10, WHITE);
} else {
canvas.drawCircle((float)blue.getCenterPx().x, (float)blue.getCenterPx().y, 10, RED);