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

Merge branch 'bumblebee_dev' of https://github.com/IronEaglesRobotics/CenterStage into bumblebee_dev

This commit is contained in:
Thomas 2023-11-17 21:09:53 -06:00
parent acb43f0940 a335c50c9b
commit 42f8c06c30
11 changed files with 133 additions and 84 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -18,68 +18,62 @@ import org.firstinspires.ftc.teamcode.hardware.Robot;
import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
import org.firstinspires.ftc.teamcode.vision.Detection;
import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
import org.opencv.core.Point;
@Config
public abstract class AutoBase extends LinearOpMode {
public static int DEPOSIT_HEIGHT = 100;
public static int SCORING_DURATION_MS = 5000;
protected Robot robot;
protected FtcDashboard dashboard;
protected Telemetry dashboardTelemetry;
protected CenterStageCommon.PropLocation propLocation;
protected final Pose2d initialPosition;
protected final CenterStageCommon.Alliance alliance;
protected final Vector2d rendezvous;
protected AutoBase(CenterStageCommon.Alliance alliance, Pose2d initialPosition, Vector2d rendezvous) {
this.alliance = alliance;
this.initialPosition = initialPosition;
this.rendezvous = rendezvous;
}
@Override
public void runOpMode() {
this.robot = new Robot(hardwareMap, telemetry);
// Initialize Robot and Dashboard
this.robot = new Robot(hardwareMap, telemetry, initialPosition, alliance);
this.dashboard = FtcDashboard.getInstance();
this.dashboardTelemetry = dashboard.getTelemetry();
this.robot.getCamera().setAlliance(CenterStageCommon.Alliance.Blue);
this.robot.getDrive().setPoseEstimate(new Pose2d(-36, 63, Math.toRadians(-90)));
// Wait for match to start
while(!isStarted() && !isStopRequested()) {
this.robot.update();
this.sleep(20);
}
setPropLocationIfVisible(Center, null);
// If the prop is visible at this point, then it must be in the center (2) position
determinePropLocation();
TrajectorySequenceBuilder builder = this.robot.getDrive()
.trajectorySequenceBuilder(new Pose2d(-36, 63, Math.toRadians(-90)));
if (this.propLocation != CenterStageCommon.PropLocation.Center) {
builder.forward(5);
builder.turn(Math.toRadians(-33));
this.robot.getDrive().followTrajectorySequence(builder.build());
setPropLocationIfVisible(Right, Left);
return;
} else {
// Center
builder.lineToConstantHeading(new Vector2d(-36, 11));
builder.addDisplacementMarker(10, () -> {
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);
switch (this.propLocation) {
case Left:
// TODO Tommy: Place the pixel on the left tape and move to rendezvous position
break;
case Unknown:
case Center:
dislodgePropAndPlacePixel();
break;
case Right:
// TODO Tommy: Place the pixel on the right tape and move to rendezvous position
break;
}
builder = this.robot.getDrive().trajectorySequenceBuilder(new Pose2d(-36, 11));
builder.lineToLinearHeading(new Pose2d(36, 11, 0));
builder.lineToLinearHeading(new Pose2d(36, 38, 0));
this.robot.getDrive().followTrajectorySequence(builder.build());
moveToBackstage();
prepareToScore();
scorePreloadedPixel();
double distance = getDistanceToAprilTag();
builder = this.robot.getDrive().trajectorySequenceBuilder(new Pose2d(36, 38, 0));
builder.forward(distance - SCORING_DISTANCE_FROM_APRIL_TAG);
this.robot.getDrive().followTrajectorySequence(builder.build());
// TODO Tommy: Park
}
private void scorePreloadedPixel() {
this.robot.getGantry().setSlideTarget(DEPOSIT_HEIGHT);
this.robot.getGantry().armOut();
while(this.robot.getGantry().isIn()) {
@ -87,33 +81,50 @@ public abstract class AutoBase extends LinearOpMode {
sleep(20);
}
this.robot.getGantry().deposit();
this.sleep(SCORING_DURATION_MS);
this.robot.getGantry().stop();
}
while(opModeIsActive()) {
this.robot.update();
AprilTagDetection aprilTagDetection = this.robot.getCamera().getAprilTag(2);
if (aprilTagDetection != null) {
Point center = aprilTagDetection.center;
this.dashboardTelemetry.addData("center", center);
this.dashboardTelemetry.addData("x", aprilTagDetection.ftcPose.x);
this.dashboardTelemetry.addData("y", aprilTagDetection.ftcPose.y);
this.dashboardTelemetry.addData("z", aprilTagDetection.ftcPose.z);
this.dashboardTelemetry.update();
}
sleep(20);
private void prepareToScore() {
double distance = this.robot.getCamera().getDistanceToAprilTag(2, 25, 5);
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.forward(distance - SCORING_DISTANCE_FROM_APRIL_TAG);
this.robot.getDrive().followTrajectorySequence(builder.build());
}
private void moveToBackstage() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(new Pose2d(36, 11, 0));
builder.lineToLinearHeading(new Pose2d(36, 38, 0));
this.robot.getDrive().followTrajectorySequence(builder.build());
}
private void dislodgePropAndPlacePixel() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToConstantHeading(rendezvous);
builder.addDisplacementMarker(10, () -> {
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 determinePropLocation() {
setPropLocationIfVisible(Center, null);
if (this.propLocation != Center) {
peekRight();
}
}
private double getDistanceToAprilTag() {
double minDistance = Double.MAX_VALUE;
for (int i = 0; i < 10; i++) {
AprilTagDetection aprilTagDetection = this.robot.getCamera().getAprilTag(2);
if (aprilTagDetection != null) {
if (aprilTagDetection.ftcPose.y < minDistance) {
minDistance = aprilTagDetection.ftcPose.y;
}
}
}
return minDistance;
private void peekRight() {
TrajectorySequenceBuilder builder = this.robot.getDrive()
.trajectorySequenceBuilder(initialPosition);
builder.forward(5);
builder.turn(Math.toRadians(-33));
this.robot.getDrive().followTrajectorySequence(builder.build());
setPropLocationIfVisible(Right, Left);
}
protected static int getExpectedAprilTagId(CenterStageCommon.PropLocation propLocation) {

11
TeamCode/src/main/java/opmodes/LeftAuto.java
View File

@ -1,9 +1,20 @@
package opmodes;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.geometry.Vector2d;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
@Autonomous(name = "Left Auto", preselectTeleOp = "MainTeleOp")
public class LeftAuto extends AutoBase {
public LeftAuto() {
super(
CenterStageCommon.Alliance.Red,
new Pose2d(-36, 63, Math.toRadians(-90)),
new Vector2d(-36, 11));
}
@Override
public void runOpMode() {
super.runOpMode();

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

@ -17,9 +17,6 @@ import org.firstinspires.ftc.vision.VisionPortal;
import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor;
import lombok.Getter;
import lombok.Setter;
public class Camera {
private PropDetectionPipeline prop;
private AprilTagProcessor aprilTag;
@ -92,6 +89,19 @@ public class Camera {
.orElse(null);
}
public double getDistanceToAprilTag(int id, double rejectAbove, double rejectBelow) {
for (int i = 0; i < 10; i++) {
AprilTagDetection aprilTagDetection = getAprilTag(id);
if (aprilTagDetection != null) {
if (aprilTagDetection.ftcPose.y < rejectAbove
&& aprilTagDetection.ftcPose.y > rejectBelow) {
return aprilTagDetection.ftcPose.y;
}
}
}
return Double.MAX_VALUE;
}
public void setAlliance(CenterStageCommon.Alliance alliance) {
this.prop.setAlliance(alliance);
}

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

@ -1,13 +1,13 @@
package org.firstinspires.ftc.teamcode.hardware;
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.CLAW_ARM_LEFT_NAME;
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.CLAW_KP;
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.PICKUP_ARM_MAX;
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.PICKUP_ARM_MIN;
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.CLAW_ARM_RIGHT_NAME;
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.CLAW_KP;
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.CLAW_MAX;
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.CLAW_MIN;
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.CLAW_NAME;
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.PICKUP_ARM_MAX;
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.PICKUP_ARM_MIN;
import com.arcrobotics.ftclib.controller.PController;
import com.qualcomm.robotcore.hardware.HardwareMap;

4
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Gantry.java
View File

@ -5,12 +5,12 @@ import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.GANTRY_ARM_KP;
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.GANTRY_ARM_MAX;
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.GANTRY_ARM_MIN;
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.GANTRY_ARM_NAME;
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.SLIDE_POWER;
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.X_CENTER;
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.GANTRY_SCREW_NAME;
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.GANTRY_X_NAME;
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.LEFT_SLIDE_MOTOR_NAME;
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.RIGHT_SLIDE_MOTOR_NAME;
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.SLIDE_POWER;
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.X_CENTER;
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.X_KP;
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.X_MAX;
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.X_MIN;

21
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java
View File

@ -4,9 +4,10 @@ import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.HardwareMap;
import org.checkerframework.checker.units.qual.C;
import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive;
import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
import lombok.Getter;
@ -27,6 +28,9 @@ public class Robot {
@Getter
private Camera camera;
@Getter
CenterStageCommon.Alliance alliance;
private final Telemetry telemetry;
public Robot(HardwareMap hardwareMap, Telemetry telemetry) {
@ -34,6 +38,12 @@ public class Robot {
this.init(hardwareMap);
}
public Robot(HardwareMap hardwareMap, Telemetry telemetry, Pose2d initialPosition, CenterStageCommon.Alliance alliance) {
this(hardwareMap, telemetry);
this.getDrive().setPoseEstimate(initialPosition);
this.setAlliance(alliance);
}
private void init(HardwareMap hardwareMap) {
this.drive = new MecanumDrive(hardwareMap);
this.drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
@ -43,6 +53,15 @@ public class Robot {
this.camera = new Camera(hardwareMap, telemetry);
}
public TrajectorySequenceBuilder getTrajectorySequenceBuilder() {
return this.drive.trajectorySequenceBuilder(this.drive.getPoseEstimate());
}
public void setAlliance(CenterStageCommon.Alliance alliance) {
this.alliance = alliance;
this.camera.setAlliance(alliance);
}
public void update() {
this.gantry.update();
this.lift.update();

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

@ -17,6 +17,9 @@ import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveCons
import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.RUN_USING_ENCODER;
import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.TRACK_WIDTH;
import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.encoderTicksToInches;
import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.kA;
import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.kStatic;
import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.kV;
import androidx.annotation.NonNull;
@ -51,10 +54,6 @@ import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.Tra
import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceRunner;
import org.firstinspires.ftc.teamcode.hardware.roadrunner.util.LynxModuleUtil;
import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.kV;
import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.kA;
import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.kStatic;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.List;

1
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/trajectorysequence/TrajectorySequenceRunner.java
View File

@ -25,7 +25,6 @@ import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.seq
import org.firstinspires.ftc.teamcode.hardware.roadrunner.util.DashboardUtil;
import org.firstinspires.ftc.teamcode.hardware.roadrunner.util.LogFiles;
import java.util.ArrayList;
import java.util.Collections;
import java.util.LinkedList;

1
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/util/LogFiles.java
View File

@ -20,7 +20,6 @@ import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants;
import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive;
import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.StandardTrackingWheelLocalizer;
import java.io.File;
import java.io.FileInputStream;
import java.io.IOException;

3
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Colors.java
View File

@ -1,7 +1,8 @@
package org.firstinspires.ftc.teamcode.util;
import org.opencv.core.Scalar;
import android.graphics.Color;
import org.opencv.core.Scalar;
public class Colors {
// CV Color Threshold Constants
public static Scalar FTC_RED_LOWER = new Scalar(165, 80, 80);

14
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/Detection.java
View File

@ -1,12 +1,5 @@
package org.firstinspires.ftc.teamcode.vision;
import org.opencv.core.Mat;
import org.opencv.core.MatOfPoint;
import org.opencv.core.Point;
import org.opencv.core.Scalar;
import org.opencv.core.Size;
import org.opencv.imgproc.Imgproc;
import static org.firstinspires.ftc.teamcode.util.Constants.GREEN;
import static org.firstinspires.ftc.teamcode.util.Constants.INVALID_AREA;
import static org.firstinspires.ftc.teamcode.util.Constants.INVALID_POINT;
@ -17,6 +10,13 @@ import static org.firstinspires.ftc.teamcode.util.OpenCVUtil.getBottomLeftOfCont
import static org.firstinspires.ftc.teamcode.util.OpenCVUtil.getBottomRightOfContour;
import static org.firstinspires.ftc.teamcode.util.OpenCVUtil.getCenterOfContour;
import org.opencv.core.Mat;
import org.opencv.core.MatOfPoint;
import org.opencv.core.Point;
import org.opencv.core.Scalar;
import org.opencv.core.Size;
import org.opencv.imgproc.Imgproc;
// Class for a Detection
public class Detection {
private double minAreaPx;