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

Working left auto

This commit is contained in:
Scott Barnes 2023-11-18 14:38:00 -06:00
parent 3f9e39e45b
commit c35ed27a09
3 changed files with 54 additions and 40 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -24,7 +24,7 @@ public abstract class AutoBase extends LinearOpMode {
public static int DEPOSIT_HEIGHT = 100;
public static double SCORING_DURATION_S = 5f;
public static double APRIL_TAG_RIGHT_DELTA = -8.5;
public static double APRIL_TAG_LEFT_DELTA = 4.0;
public static double APRIL_TAG_LEFT_DELTA = 8.0;
protected Robot robot;
protected FtcDashboard dashboard;
@ -59,8 +59,12 @@ public abstract class AutoBase extends LinearOpMode {
TrajectorySequenceBuilder builder;
switch (this.propLocation) {
case Left:
// TODO Tommy: Place the pixel on the left tape and move to rendezvous position
return;
dislodgePropAndPlacePixelLeft();
builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(rendezvous);
this.robot.getDrive().followTrajectorySequence(builder.build());
break;
case Unknown:
case Center:
dislodgePropAndPlacePixelCenter();
@ -85,6 +89,19 @@ public abstract class AutoBase extends LinearOpMode {
// TODO Tommy: Park
}
private void dislodgePropAndPlacePixelLeft() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(new Pose2d(-52, 31, Math.toRadians(-180)));
builder.lineToConstantHeading(new Vector2d(-42, 31));
builder.addTemporalMarker(0.2, () -> {
this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN);
});
this.robot.getDrive().followTrajectorySequence(builder.build());
this.robot.getClaw().openSync();
this.sleep(100);
this.robot.getClaw().setArmPosition(PICKUP_ARM_MAX);
}
private void dislodgePropAndPlacePixelRight() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();

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

@ -1,11 +1,9 @@
package org.firstinspires.ftc.teamcode.hardware;
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.hardware.RobotConfig.WEBCAM_NAME;
import static org.firstinspires.ftc.teamcode.util.Constants.INVALID_DETECTION;
import com.acmerobotics.dashboard.config.Config;
import com.qualcomm.robotcore.hardware.HardwareMap;
import org.firstinspires.ftc.robotcore.external.Telemetry;
@ -16,8 +14,11 @@ import org.firstinspires.ftc.teamcode.vision.PropDetectionPipeline;
import org.firstinspires.ftc.vision.VisionPortal;
import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor;
import org.opencv.core.Point;
@Config
public class Camera {
public static float PROP_REJECTION_VERTICAL = 300;
private PropDetectionPipeline prop;
private AprilTagProcessor aprilTag;
private VisionPortal visionPortal;
@ -48,39 +49,28 @@ public class Camera {
return INVALID_DETECTION;
}
Detection detection = null;
switch (getAlliance()) {
case Blue:
Detection blue = this.prop.getBlue();
return blue != null && blue.isValid() ? blue : INVALID_DETECTION;
detection = blue != null && blue.isValid() ? blue : INVALID_DETECTION;
break;
case Red:
Detection red = this.prop.getRed();
return red != null && red.isValid() ? red : INVALID_DETECTION;
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 INVALID_DETECTION;
}
public CenterStageCommon.PropLocation getPropLocation() {
Detection prop = this.getProp();
if (prop == null || !prop.isValid()) {
return CenterStageCommon.PropLocation.Unknown;
}
double x = prop.getCenter().x + 50;
if (x <= DETECTION_LEFT_X) {
return CenterStageCommon.PropLocation.Left;
}
if (x <= DETECTION_CENTER_X) {
return CenterStageCommon.PropLocation.Center;
}
if (x <= DETECTION_RIGHT_X) {
return CenterStageCommon.PropLocation.Right;
}
return CenterStageCommon.PropLocation.Unknown;
}
public AprilTagDetection getAprilTag(int id) {
return this.aprilTag.getDetections()
.stream()

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

@ -1,5 +1,6 @@
package org.firstinspires.ftc.teamcode.vision;
import static org.firstinspires.ftc.teamcode.hardware.Camera.PROP_REJECTION_VERTICAL;
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;
@ -8,6 +9,7 @@ import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.DETECTION_RIGH
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.BLUR_SIZE;
@ -24,6 +26,7 @@ import org.firstinspires.ftc.vision.VisionProcessor;
import org.opencv.core.Core;
import org.opencv.core.Mat;
import org.opencv.core.MatOfPoint;
import org.opencv.core.Point;
import org.opencv.core.Size;
import org.opencv.imgproc.Imgproc;
@ -99,20 +102,24 @@ public class PropDetectionPipeline implements VisionProcessor {
@Override
public void onDrawFrame(Canvas canvas, int onscreenWidth, int onscreenHeight, float scaleBmpPxToCanvasPx, float scaleCanvasDensity, Object userContext) {
int detectionLeftXPx = (int)((DETECTION_LEFT_X / 100) * onscreenWidth);
int detectionCenterXPx = (int)((DETECTION_CENTER_X / 100) * onscreenWidth);
int detectionRightXPx = (int)((DETECTION_RIGHT_X / 100) * onscreenWidth);
canvas.drawLine(0, PROP_REJECTION_VERTICAL, canvas.getWidth(), PROP_REJECTION_VERTICAL, WHITE);
canvas.drawLine(detectionLeftXPx, 0, detectionLeftXPx, canvas.getHeight(), WHITE);
canvas.drawLine(detectionCenterXPx, 0, detectionCenterXPx, canvas.getHeight(), WHITE);
canvas.drawLine(detectionRightXPx, 0, detectionRightXPx, canvas.getHeight(), WHITE);
if (red.isValid()) {
canvas.drawCircle((float)red.getCenterPx().x, (float)red.getCenterPx().y, 10, WHITE);
if (red != null && red.isValid()) {
Point center = red.getCenterPx();
if (center.y < PROP_REJECTION_VERTICAL) {
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);
}
}
if (blue.isValid()) {
canvas.drawCircle((float)blue.getCenterPx().x, (float)blue.getCenterPx().y, 10, WHITE);
if (blue != null && blue.isValid()) {
Point center = blue.getCenterPx();
if (center.y < PROP_REJECTION_VERTICAL) {
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);
}
}
}
}