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

Red and Blue auto works, Teleop works, everything works.

This commit is contained in:
sihan 2023-11-08 20:40:33 -06:00
parent 655f41219e
commit 420fa25c31
4 changed files with 235 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/Auto.java
View File

@ -58,7 +58,7 @@ public class Auto extends LinearOpMode {
.build();
this.boardOne = this.robot.getDrive().trajectoryBuilder(scoreOne.end())
.lineToLinearHeading(new Pose2d(72, -26, Math.toRadians(360)))
.lineToLinearHeading(new Pose2d(73, -26, Math.toRadians(360)))
.addTemporalMarker(.2, robot.getArm()::armAccurateScore)
.addTemporalMarker(.2, robot.getWrist()::wristScore)
.build();
@ -74,7 +74,7 @@ public class Auto extends LinearOpMode {
.build();
this.scoreTwo = this.robot.getDrive().trajectoryBuilder(preloadTwo.end())
.lineToLinearHeading(new Pose2d(72, -34, Math.toRadians(360)))
.lineToLinearHeading(new Pose2d(73, -34, Math.toRadians(360)))
.addTemporalMarker(.2, robot.getArm()::armAccurateScore)
.addTemporalMarker(.2, robot.getWrist()::wristScore)
.build();
@ -89,7 +89,7 @@ public class Auto extends LinearOpMode {
.build();
this.scoreThree = this.robot.getDrive().trajectoryBuilder(preloadThree.end())
.lineToLinearHeading(new Pose2d(72, -42, Math.toRadians(360)))
.lineToLinearHeading(new Pose2d(73, -42, Math.toRadians(360)))
.addTemporalMarker(.2, robot.getArm()::armAccurateScore)
.addTemporalMarker(.2, robot.getWrist()::wristScore)
.build();

154
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBlue.java Normal file
View File

@ -0,0 +1,154 @@
package org.firstinspires.ftc.teamcode.opmodes;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.trajectory.Trajectory;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.teamcode.hardware.Robot;
@Autonomous(name = "autoBlue")
public class AutoBlue extends LinearOpMode {
protected Pose2d initialPosition;
protected Trajectory preloadOne;
protected Trajectory scoreOne;
protected Trajectory boardOne;
protected Trajectory backOffOne;
protected Trajectory preloadTwo;
protected Trajectory scoreTwo;
protected Trajectory backOffTwo;
protected Trajectory preloadThree;
protected Trajectory scoreThree;
protected Trajectory backOffThree;
protected Trajectory park1;
protected Trajectory park2;
private Robot robot;
private String randomization;
private int random;
@Override
public void runOpMode() throws InterruptedException {
this.telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
this.robot = new Robot().init(hardwareMap);
this.robot.getCamera().initTargetingCamera();
//Trajectories
this.initialPosition = new Pose2d(-34, -59.5, Math.toRadians(270));
this.robot.getDrive().setPoseEstimate(initialPosition);
//Randomization One
this.preloadOne = this.robot.getDrive().trajectoryBuilder(initialPosition)
.lineToLinearHeading(new Pose2d(-40, -37.5, Math.toRadians(270)))
.build();
this.scoreOne = this.robot.getDrive().trajectoryBuilder(preloadOne.end())
.lineToLinearHeading(new Pose2d(-29, -30, Math.toRadians(180)))
.build();
this.boardOne = this.robot.getDrive().trajectoryBuilder(scoreOne.end())
.lineToLinearHeading(new Pose2d(-72, -26, Math.toRadians(180)))
.addTemporalMarker(.2, robot.getArm()::armAccurateScore)
.addTemporalMarker(.2, robot.getWrist()::wristScore)
.build();
this.backOffOne = this.robot.getDrive().trajectoryBuilder(boardOne.end())
.lineToLinearHeading(new Pose2d(-60, -26, Math.toRadians(180)))
.build();
//Randomization Two
this.preloadTwo = this.robot.getDrive().trajectoryBuilder(initialPosition)
.lineToLinearHeading(new Pose2d(-35, -27, Math.toRadians(270)))
.build();
this.scoreTwo = this.robot.getDrive().trajectoryBuilder(preloadTwo.end())
.lineToLinearHeading(new Pose2d(-70, -34, Math.toRadians(180)))
.addTemporalMarker(.2, robot.getArm()::armAccurateScore)
.addTemporalMarker(.2, robot.getWrist()::wristScore)
.build();
this.backOffTwo = this.robot.getDrive().trajectoryBuilder(scoreTwo.end())
.lineToLinearHeading(new Pose2d(-60, -34, Math.toRadians(180)))
.build();
//Randomization Three
this.preloadThree = this.robot.getDrive().trajectoryBuilder(initialPosition)
.lineToLinearHeading(new Pose2d(-42, -35, Math.toRadians(270)))
.build();
this.scoreThree = this.robot.getDrive().trajectoryBuilder(preloadThree.end())
.lineToLinearHeading(new Pose2d(-71.5, -41.3, Math.toRadians(180)))
.addTemporalMarker(.2, robot.getArm()::armAccurateScore)
.addTemporalMarker(.2, robot.getWrist()::wristScore)
.build();
this.backOffThree = this.robot.getDrive().trajectoryBuilder(scoreThree.end())
.lineToLinearHeading(new Pose2d(-60, -40, Math.toRadians(180)))
.build();
//Park
this.park1 = this.robot.getDrive().trajectoryBuilder(backOffTwo.end())
.lineToLinearHeading(new Pose2d(-65, -10, Math.toRadians(180)))
.addTemporalMarker(.3, robot.getArm()::armRest)
.addTemporalMarker(.3, robot.getWrist()::wristPickup)
.build();
this.park2 = this.robot.getDrive().trajectoryBuilder(park1.end())
.lineToLinearHeading(new Pose2d(-80, -10, Math.toRadians(180)))
.build();
// Do super fancy chinese shit
while (!this.isStarted()) {
this.telemetry.addData("Starting Position", this.robot.getCamera().getStartingPositionBlue());
randomization = String.valueOf(this.robot.getCamera().getStartingPositionBlue());
this.telemetry.update();
}
switch (randomization) {
case "RIGHT":
this.robot.getDrive().followTrajectory(preloadOne);
this.robot.getDrive().followTrajectory(scoreOne);
this.robot.getDrive().followTrajectory(boardOne);
this.robot.getClaw().open();
sleep(500);
this.robot.getDrive().followTrajectory(backOffOne);
sleep(500);
this.robot.getDrive().followTrajectory(park1);
this.robot.getDrive().followTrajectory(park2);
break;
case "CENTER":
this.robot.getDrive().followTrajectory(preloadTwo);
this.robot.getDrive().followTrajectory(scoreTwo);
this.robot.getClaw().open();
sleep(500);
this.robot.getDrive().followTrajectory(backOffTwo);
sleep(500);
this.robot.getDrive().followTrajectory(park1);
this.robot.getDrive().followTrajectory(park2);
break;
case "LEFT":
this.robot.getDrive().followTrajectory(preloadThree);
this.robot.getDrive().followTrajectory(scoreThree);
this.robot.getClaw().open();
sleep(500);
this.robot.getDrive().followTrajectory(backOffThree);
sleep(500);
this.robot.getDrive().followTrajectory(park1);
this.robot.getDrive().followTrajectory(park2);
break;
}
}
}

38
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/Camera.java
View File

@ -79,11 +79,49 @@ public class Camera {
return StarterPosition.RIGHT;
}
return StarterPosition.UNKNOWN;
}
public Detection getBlue() {
return targetingCameraInitialized ? targetingPipeline.getBlue() : INVALID_DETECTION;
}
public StarterPositionBlue getStartingPositionBlue() {
if (!targetingCameraInitialized) {
return StarterPositionBlue.UNKNOWN;
}
Detection detection = this.getBlue();
if (detection == null || !detection.isValid()) {
return StarterPositionBlue.UNKNOWN;
}
Point center = detection.getCenter();
if (center == null) {
return StarterPositionBlue.UNKNOWN;
}
double centerX = this.getBlue().getCenter().x + 50;
if (centerX < 33) {
return StarterPositionBlue.LEFT;
} else if (centerX < 66) {
return StarterPositionBlue.CENTER;
} else if (centerX < 100) {
return StarterPositionBlue.RIGHT;
}
return StarterPositionBlue.UNKNOWN;
}
public enum StarterPosition {
UNKNOWN, LEFT, CENTER, RIGHT
}
public enum StarterPositionBlue {
UNKNOWN, LEFT, CENTER, RIGHT
}
}

40
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/ColorDetectionPipeline.java
View File

@ -1,8 +1,11 @@
package org.firstinspires.ftc.teamcode.vision;
import static org.firstinspires.ftc.teamcode.util.Configurables.FTC_BLUE_LOWER;
import static org.firstinspires.ftc.teamcode.util.Configurables.FTC_BLUE_UPPER;
import static org.firstinspires.ftc.teamcode.util.Configurables.FTC_RED_LOWER;
import static org.firstinspires.ftc.teamcode.util.Configurables.FTC_RED_UPPER;
import static org.firstinspires.ftc.teamcode.util.Constants.ANCHOR;
import static org.firstinspires.ftc.teamcode.util.Constants.BLUE;
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.RED;
@ -30,14 +33,26 @@ public class ColorDetectionPipeline extends OpenCvPipeline {
Scalar redGoalLower2;
Scalar redGoalUpper2;
Mat blueMask1 = new Mat();
Mat blueMask2 = new Mat();
Mat blueMask = new Mat();
Scalar blueGoalLower1;
Scalar blueGoalUpper1;
Scalar blueGoalLower2;
Scalar blueGoalUpper2;
private Detection red;
private Detection blue;
// Init
@Override
public void init(Mat input) {
red = new Detection(input.size(), 0);
blue = new Detection(input.size(), 0);
}
// Process each frame that is received from the webcam
@Override
public Mat processFrame(Mat input) {
@ -45,6 +60,7 @@ public class ColorDetectionPipeline extends OpenCvPipeline {
Imgproc.cvtColor(blurred, hsv, Imgproc.COLOR_RGB2HSV);
updateRed(input);
updateBlue(input);
return input;
}
@ -74,7 +90,31 @@ public class ColorDetectionPipeline extends OpenCvPipeline {
red.fill(input, RED);
}
private void updateBlue(Mat input) {
// take pixels that are in the color range and put them into a mask, eroding and dilating them to remove white noise
blueGoalLower1 = new Scalar(FTC_BLUE_LOWER.getH(), FTC_BLUE_LOWER.getS(), FTC_BLUE_LOWER.getV());
blueGoalUpper1 = new Scalar(FTC_BLUE_UPPER.getH(), FTC_BLUE_UPPER.getS(), FTC_BLUE_UPPER.getV());
Core.inRange(hsv, blueGoalLower1, blueGoalUpper1, blueMask);
Imgproc.erode(blueMask, blueMask, STRUCTURING_ELEMENT, ANCHOR, ERODE_DILATE_ITERATIONS);
Imgproc.dilate(blueMask, blueMask, STRUCTURING_ELEMENT, ANCHOR, ERODE_DILATE_ITERATIONS);
ArrayList<MatOfPoint> contours = new ArrayList<>();
Imgproc.findContours(blueMask, contours, new Mat(), Imgproc.RETR_LIST, Imgproc.CHAIN_APPROX_SIMPLE);
for (int i = 0; i < contours.size(); i++) {
Detection newDetection = new Detection(input.size(), 0, 1f);
newDetection.setContour(contours.get(i));
newDetection.draw(input, BLUE);
}
blue.setContour(getLargestContour(contours));
blue.fill(input, BLUE);
}
public Detection getRed() {
return this.red;
}
public Detection getBlue() {
return this.blue;
}
}