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

Camera, Autos, Autoalign etc

This commit is contained in:
Thomas 2024-01-20 12:34:42 -06:00
parent ea7cfae4f6
commit 4858eabe16
11 changed files with 154 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -23,7 +23,7 @@ import org.firstinspires.ftc.teamcode.vision.Detection;
@Config
public abstract class AutoBase extends LinearOpMode {
public static int DEPOSIT_HEIGHT = 100;
public static double SCORING_DURATION_S = 5f;
public static double SCORING_DURATION_S = 3f; // for spin of axle
public static double APRIL_TAG_RIGHT_DELTA = -8.5;
public static double APRIL_TAG_LEFT_DELTA = 7.0;
protected static double Delay = 5000;
@ -55,7 +55,7 @@ public abstract class AutoBase extends LinearOpMode {
this.robot.update();
this.sleep(20);
}
if (isStopRequested()) {
if (isStopRequested()) { // remove later if nessacary as recent addition might be interefering
return;
}
@ -130,7 +130,9 @@ public abstract class AutoBase extends LinearOpMode {
break;
}
double distance = this.robot.getCamera().getDistanceToAprilTag(this.alliance == CenterStageCommon.Alliance.Blue ? 2:5, 25, 5);
Vector2d target = new Vector2d(this.robot.getDrive().getPoseEstimate().getX() + (distance - SCORING_DISTANCE_FROM_APRIL_TAG), this.robot.getDrive().getPoseEstimate().getY() + delta);
Vector2d target = new Vector2d(this.robot.getDrive().getPoseEstimate().getX() +
(distance - SCORING_DISTANCE_FROM_APRIL_TAG),
this.robot.getDrive().getPoseEstimate().getY() + delta);
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToConstantHeading(target);
this.robot.getDrive().followTrajectorySequence(builder.build());

73
TeamCode/src/main/java/opmodes/AutoSandbox.java Normal file
View File

@ -0,0 +1,73 @@
package opmodes;
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.PICKUP_ARM_MIN;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.geometry.Vector2d;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
@Autonomous(name = "AutoSandbox", preselectTeleOp = "MainTeleOp")
public class AutoSandbox extends AutoBase {
private final Pose2d rendezvous = new Pose2d(12, 48, Math.toRadians(-89));
public AutoSandbox() {
super(
CenterStageCommon.Alliance.Blue,
new Pose2d(12, 63, Math.toRadians(-90)),
new Pose2d(62, 62));
}
protected void propLeft() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(new Pose2d(36, 25, Math.toRadians(-33)));
builder.addDisplacementMarker(10, () -> {
this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN);
});
this.robot.getDrive().followTrajectorySequence(builder.build());
openAndLiftClaw();
}
protected void propCenter() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(new Pose2d(12,48, Math.toRadians(90)));
builder.addDisplacementMarker(10, () -> {
this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN);
});
builder.lineToConstantHeading(new Vector2d(12,12));
this.robot.getDrive().followTrajectorySequence(builder.build());
openAndLiftClaw();
// builder = this.robot.getTrajectorySequenceBuilder();
// builder.turn((Math.toRadians(90)));
// this.robot.getDrive().followTrajectorySequence(builder.build());
// TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
// builder.lineToConstantHeading(rendezvous.vec());
// builder.addDisplacementMarker(10, () -> {
// this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN);
// });
// this.robot.getDrive().followTrajectorySequence(builder.build());
//
// openAndLiftClaw();
}
protected void propRight() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(new Pose2d(32, 34, Math.toRadians(0)));
builder.lineToConstantHeading(new Vector2d(19, 34));
builder.addTemporalMarker(0.5, () -> {
this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN);
});
this.robot.getDrive().followTrajectorySequence(builder.build());
openAndLiftClaw();
}
}

2
TeamCode/src/main/java/opmodes/BlueBackStage.java
View File

@ -11,7 +11,7 @@ import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
@Autonomous(name = "BlueBackStage", preselectTeleOp = "MainTeleOp")
public class BlueBackStage extends AutoBase {
private final Pose2d rendezvous = new Pose2d(12, 11);
private final Pose2d rendezvous = new Pose2d(12, 10);
public BlueBackStage() {
super(

2
TeamCode/src/main/java/opmodes/BlueFrontStage.java
View File

@ -24,7 +24,7 @@ public class BlueFrontStage extends AutoBase {
this.sleep(5000);
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(new Pose2d(-52, 31, Math.toRadians(-180)));
builder.lineToConstantHeading(new Vector2d(-42, 31));
builder.lineToConstantHeading(new Vector2d(-40, 31));
builder.addTemporalMarker(0.2, () -> {
this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN);
});

43
TeamCode/src/main/java/opmodes/MainTeleOp.java
View File

@ -33,9 +33,9 @@ public class MainTeleOp extends OpMode {
private boolean liftArmShouldBeUp = false;
private boolean screwArmIsMoving = false;
private FtcDashboard dashboard;
private boolean leftWasPressed;
private boolean rightWasPressed;
private boolean isGantryFlipped = false;
private int targetTagId;
@Override
public void init() {
this.dashboard = FtcDashboard.getInstance();
@ -122,6 +122,7 @@ public class MainTeleOp extends OpMode {
this.robot.getGantry().setSlideTarget(SLIDE_UP);
} else if (slideDown) {
this.robot.getGantry().setSlideTarget(0);
this.robot.getGantry().stop();
} else if (previousSlideUp || previousSlideDown) {
this.robot.getGantry().setSlideTarget(this.robot.getGantry().getSlidePosition());
} else if (!isGantryFlipped)
@ -168,22 +169,25 @@ public class MainTeleOp extends OpMode {
}
Vector2d poseFromAprilTag = this.robot.getCamera().getPoseFromAprilTag(2, 5);
dashboard.getTelemetry().addData("Inferred Position", poseFromAprilTag);
dashboard.getTelemetry().update();
if (poseFromAprilTag != null) {
if (leftPressed && !leftWasPressed) {
if (leftPressed) {
macroToScore(poseFromAprilTag, true);
} else if (rightPressed && !rightWasPressed) {
} else if (rightPressed) {
macroToScore(poseFromAprilTag, false);
}
} else {
if (leftPressed || rightPressed) {
moveToStartSquare();
}
}
if (!leftPressed && !rightPressed) { // breaks drive as makes it so it continuously brakes, added !this.robot.getDrive().isBusy just for placement
if (!leftPressed && !rightPressed) {
this.robot.getDrive().breakFollowing();
}
this.leftWasPressed = leftPressed;
this.rightWasPressed = rightPressed;
this.previousSlideUp = slideUp;
this.previousScrewArmToggle = screwArmToggle;
this.previousRobotLiftReset = robotLiftReset;
@ -192,7 +196,30 @@ public class MainTeleOp extends OpMode {
this.robot.update();
}
private void moveToStartSquare() {
if (this.robot.getDrive().isBusy()) {
return;
}
Pose2d currentPoseEstimate = this.robot.getDrive().getPoseEstimate();
if (currentPoseEstimate.getX() < 0) {
return;
}
this.targetTagId = currentPoseEstimate.getY() >= 0 ? 2 : 5;
double y = targetTagId == 2 ? 36f : -36f;
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(new Pose2d(34, y, 0));
this.robot.getDrive().followTrajectorySequenceAsync(builder.build());
}
private void macroToScore(Vector2d poseFromAprilTag, boolean left) {
if (this.robot.getDrive().isBusy()) {
return;
}
Pose2d target; // defines a new pose2d named target, position not yet given
Pose2d poseEstimate = new Pose2d(poseFromAprilTag.getX(), poseFromAprilTag.getY(), this.robot.getDrive().getPoseEstimate().getHeading());
double y = poseEstimate.getY() > 0

10
TeamCode/src/main/java/opmodes/RedBackStage.java
View File

@ -11,19 +11,19 @@ import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
@Autonomous(name = "RedBackStage", preselectTeleOp = "MainTeleOp")
public class RedBackStage extends AutoBase {
private final Pose2d rendezvous = new Pose2d(12, 11);
private final Pose2d rendezvous = new Pose2d(12, -11);
public RedBackStage() {
super(
CenterStageCommon.Alliance.Red,
new Pose2d(12, 63, Math.toRadians(-90)),
new Pose2d(12, -63, Math.toRadians(90)),
new Pose2d(62, -62));
}
protected void propLeft() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(new Pose2d(32, 34, Math.toRadians(0)));
builder.lineToConstantHeading(new Vector2d(19, 34));
builder.lineToLinearHeading(new Pose2d(32, -34, Math.toRadians(0)));
builder.lineToConstantHeading(new Vector2d(19, -34));
builder.addTemporalMarker(0.5, () -> {
this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN);
});
@ -50,7 +50,7 @@ public class RedBackStage extends AutoBase {
protected void propRight() {
// red back side
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(new Pose2d(36, -25, Math.toRadians(-33)));
builder.lineToLinearHeading(new Pose2d(36, -25, Math.toRadians(33)));
builder.addDisplacementMarker(10, () -> {
this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN);
});

15
TeamCode/src/main/java/opmodes/Test.java
View File

@ -37,6 +37,10 @@ public class Test extends OpMode {
public void loop() {
robot.update();
// Drive
boolean slowmode = gamepad1.right_bumper || gamepad1.y;
this.robot.getDrive().setInput(gamepad1, gamepad2, slowmode);
Vector2d poseFromAprilTag = this.robot.getCamera().getPoseFromAprilTag(2, 5);
dashboard.getTelemetry().addData("Inferred Position", poseFromAprilTag);
dashboard.getTelemetry().update();
@ -44,13 +48,13 @@ public class Test extends OpMode {
boolean leftPressed = gamepad1.dpad_left;
boolean rightPressed = gamepad1.dpad_right;
if (poseFromAprilTag != null) {
if (leftPressed && !leftWasPressed) {
if (leftPressed) {
macroToScore(poseFromAprilTag, true);
} else if (rightPressed && !rightWasPressed) {
} else if (rightPressed) {
macroToScore(poseFromAprilTag, false);
}
} else {
if (leftPressed && !leftWasPressed || rightPressed && !rightWasPressed) {
if (leftPressed || rightPressed) {
moveToStartSquare();
}
}
@ -78,11 +82,14 @@ public class Test extends OpMode {
double y = targetTagId == 2 ? 36f : -36f;
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(new Pose2d(36, y, 0));
builder.lineToLinearHeading(new Pose2d(34, y, 0));
this.robot.getDrive().followTrajectorySequenceAsync(builder.build());
}
private void macroToScore(Vector2d poseFromAprilTag, boolean left) {
if (this.robot.getDrive().isBusy()) {
return;
}
Pose2d target;
Pose2d poseEstimate = new Pose2d(poseFromAprilTag.getX(), poseFromAprilTag.getY(), this.robot.getDrive().getPoseEstimate().getHeading());
double y = poseEstimate.getY() > 0

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

@ -32,6 +32,8 @@ import java.util.Arrays;
public class Camera {
public static float PROP_REJECTION_VERTICAL_UPPER = 150;
public static float PROP_REJECTION_VERTICAL_LOWER = 275;
public static float PROP_REJECTION_HORIZONTAL_LEFT = 50;
public static float PROP_REJECTION_HORIZONTAL_RIGHT = 480 - PROP_REJECTION_HORIZONTAL_LEFT;
private PropDetectionPipeline prop;
private AprilTagProcessor aprilTag;
private VisionPortal visionPortal;
@ -122,11 +124,11 @@ public class Camera {
switch (aprilTagDetection.id) {
case 2:
ourPoseX = tag2Pose.getX() - ftcPose.y - FORWARD_OFFSET_IN;
ourPoseY = tag2Pose.getY() - ftcPose.x - SIDE_OFFSET_IN;
ourPoseY = tag2Pose.getY() + ftcPose.x - SIDE_OFFSET_IN;
break;
case 5:
ourPoseX = tag5Pose.getX() - ftcPose.y - FORWARD_OFFSET_IN;
ourPoseY = tag5Pose.getY() - ftcPose.x - SIDE_OFFSET_IN;
ourPoseY = tag5Pose.getY() + ftcPose.x - SIDE_OFFSET_IN;
break;
default:
ourPoseX = 0;

8
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotConfig.java
View File

@ -32,9 +32,9 @@ public class RobotConfig {
// Arm
public static double PICKUP_ARM_MIN = 0.185;
public static double PICKUP_ARM_MAX = 0.755;
public static double CLAW_MIN = 0.92;
public static double CLAW_MAX = 0.6;
public static double PICKUP_ARM_MAX = 0.760;
public static double CLAW_MIN = 0.89;
public static double CLAW_MAX = 0.65;
public static double CLAW_ARM_DELTA = 0.03;
public static double CLAW_KP = 0.3;
@ -71,7 +71,7 @@ public class RobotConfig {
public static double DETECTION_LEFT_X = 0;
public static double DETECTION_CENTER_X = .5;
public static double DETECTION_RIGHT_X = 1;
public static double SCORING_DISTANCE_FROM_APRIL_TAG = 6f;
public static double SCORING_DISTANCE_FROM_APRIL_TAG = 7f;
public static final double FORWARD_OFFSET_IN = 7.75;
public static final double SIDE_OFFSET_IN = 0.5;
}

6
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/MecanumDrive.java
View File

@ -208,8 +208,9 @@ public class MecanumDrive extends com.acmerobotics.roadrunner.drive.MecanumDrive
}
public void breakFollowing() {
boolean wasBusy = trajectorySequenceRunner.isBusy();
trajectorySequenceRunner.breakFollowing();
if (trajectorySequenceRunner.isBusy()) {
if (wasBusy) {
setWeightedDrivePower(new Pose2d());
}
}
@ -333,6 +334,9 @@ public class MecanumDrive extends com.acmerobotics.roadrunner.drive.MecanumDrive
}
public void setInput(Gamepad gamepad1, Gamepad gamepad2, boolean slowmode) {
if (isBusy()) {
return;
}
double speedScale = slowmode ? SLOW_MODE_SPEED_PCT : SPEED;
double turnScale = slowmode ? SLOW_MODE_TURN_PCT : TURN;

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

@ -1,9 +1,13 @@
package org.firstinspires.ftc.teamcode.vision;
import static org.firstinspires.ftc.teamcode.hardware.Camera.PROP_REJECTION_HORIZONTAL_LEFT;
import static org.firstinspires.ftc.teamcode.hardware.Camera.PROP_REJECTION_HORIZONTAL_RIGHT;
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_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;
@ -19,6 +23,8 @@ import static org.firstinspires.ftc.teamcode.util.OpenCVUtil.getLargestContour;
import android.graphics.Canvas;
import org.firstinspires.ftc.robotcore.internal.camera.calibration.CameraCalibration;
import org.firstinspires.ftc.teamcode.hardware.Camera;
import org.firstinspires.ftc.teamcode.hardware.Gantry;
import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
import org.firstinspires.ftc.teamcode.util.ScalarRange;
import org.firstinspires.ftc.vision.VisionProcessor;
@ -33,6 +39,7 @@ import java.util.ArrayList;
import lombok.Getter;
import lombok.Setter;
import opmodes.Test;
public class PropDetectionPipeline implements VisionProcessor {
@Getter
@ -92,7 +99,9 @@ public class PropDetectionPipeline implements VisionProcessor {
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.rectangle(mask, new Point(0,0), new Point(PROP_REJECTION_HORIZONTAL_LEFT, mask.rows() - 1), BLACK, -1);
Imgproc.rectangle(mask, new Point(PROP_REJECTION_HORIZONTAL_RIGHT, 0), 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);