Skip to content
This repository was archived by the owner on Sep 12, 2023. It is now read-only.

Commit 6dbda12

Browse files
11981591198159
committed
workin on stuff
1 parent f29b4c5 commit 6dbda12

File tree

13 files changed

+305
-68
lines changed

13 files changed

+305
-68
lines changed

‎TechnoOneCode/src/main/java/org/firstinspires/ftc/teamcode/commands/cap/CapVariableCommand.java

+35
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,35 @@
1+
package org.firstinspires.ftc.teamcode.commands.cap;
2+
3+
import com.technototes.library.command.Command;
4+
5+
import org.firstinspires.ftc.teamcode.subsystems.CapSubsystem;
6+
7+
import java.util.function.DoubleSupplier;
8+
9+
public class CapVariableCommand implements Command {
10+
public CapSubsystem subsystem;
11+
public DoubleSupplier supplier;
12+
public CapVariableCommand(CapSubsystem s, DoubleSupplier pos){
13+
subsystem = s;
14+
supplier = pos;
15+
addRequirements(s);
16+
}
17+
public CapVariableCommand(CapSubsystem s, double pos){
18+
this(s, ()->pos);
19+
}
20+
21+
@Override
22+
public void execute() {
23+
subsystem.setCap(supplier.getAsDouble());
24+
}
25+
26+
@Override
27+
public boolean isFinished() {
28+
return false;
29+
}
30+
31+
@Override
32+
public void end(boolean cancel) {
33+
if(cancel) subsystem.restCap();
34+
}
35+
}

‎TechnoOneCode/src/main/java/org/firstinspires/ftc/teamcode/commands/carousel/CarouselLeftCommand.java

+11
Original file line numberDiff line numberDiff line change
@@ -17,5 +17,16 @@ public CarouselLeftCommand(CarouselSubsystem s){
1717
public void execute(){
1818
subsystem.left();
1919
}
20+
21+
22+
@Override
23+
public boolean isFinished() {
24+
return false;
25+
}
26+
27+
@Override
28+
public void end(boolean cancel) {
29+
subsystem.stop();
30+
}
2031
}
2132

‎TechnoOneCode/src/main/java/org/firstinspires/ftc/teamcode/commands/carousel/CarouselRightCommand.java

+10
Original file line numberDiff line numberDiff line change
@@ -17,4 +17,14 @@ public CarouselRightCommand(CarouselSubsystem s){
1717
public void execute(){
1818
subsystem.right();
1919
}
20+
21+
@Override
22+
public boolean isFinished() {
23+
return false;
24+
}
25+
26+
@Override
27+
public void end(boolean cancel) {
28+
subsystem.stop();
29+
}
2030
}

‎TechnoOneCode/src/main/java/org/firstinspires/ftc/teamcode/commands/intake/IntakeInCommand.java

+4-4
Original file line numberDiff line numberDiff line change
@@ -17,8 +17,8 @@ public void execute() {
1717
subsystem.in();
1818
}
1919

20-
// @Override
21-
// public void end(boolean cancel) {
22-
// if(cancel) subsystem.stop();
23-
// }
20+
@Override
21+
public void end(boolean cancel) {
22+
if(cancel) subsystem.stop();
23+
}
2424
}

‎TechnoOneCode/src/main/java/org/firstinspires/ftc/teamcode/commands/intake/IntakeOutCommand.java

+9-9
Original file line numberDiff line numberDiff line change
@@ -16,14 +16,14 @@ public void execute() {
1616
subsystem.out();
1717
}
1818

19-
// @Override
20-
// public boolean isFinished() {
21-
// return false;
22-
// }
23-
//
24-
// @Override
25-
// public void end(boolean cancel) {
26-
// if(cancel) subsystem.stop();
27-
// }
19+
@Override
20+
public boolean isFinished() {
21+
return false;
22+
}
23+
24+
@Override
25+
public void end(boolean cancel) {
26+
subsystem.stop();
27+
}
2828

2929
}

‎TechnoOneCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/DiffyOpMode.java

-44
This file was deleted.

‎TechnoOneCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/BarcodePipeline.java

+167
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,167 @@
1+
package org.firstinspires.ftc.teamcode.subsystems;
2+
3+
import org.firstinspires.ftc.robotcore.external.Telemetry;
4+
import org.opencv.core.Core;
5+
import org.opencv.core.Mat;
6+
import org.opencv.core.MatOfPoint;
7+
import org.opencv.core.MatOfPoint2f;
8+
import org.opencv.core.Point;
9+
import org.opencv.core.Rect;
10+
import org.opencv.core.Scalar;
11+
import org.opencv.core.Size;
12+
import org.opencv.imgproc.Imgproc;
13+
import org.openftc.easyopencv.OpenCvPipeline;
14+
15+
import java.util.ArrayList;
16+
import java.util.List;
17+
import java.util.function.Supplier;
18+
import com.acmerobotics.dashboard.config.Config;
19+
20+
import static org.firstinspires.ftc.teamcode.subsystems.BarcodePipeline.BarcodeConstants.*;
21+
public class BarcodePipeline extends OpenCvPipeline implements Supplier<Integer> {
22+
@Config
23+
public static class BarcodeConstants {
24+
public static boolean DISPLAY = true;
25+
public static Scalar DISPLAY_COLOR = new Scalar(200, 0, 0);
26+
public static Scalar LOWER_LIMIT = new Scalar( 100.0, 0.0, 0.0, 0.0);
27+
public static Scalar UPPER_LIMIT = new Scalar(255.0, 80.0, 80.0, 255.0);
28+
public static int BORDER_LEFT_X = 0; //amount of pixels from the left side of the cam to skip
29+
public static int BORDER_RIGHT_X = 0; //amount of pixels from the right of the cam to skip
30+
public static int BORDER_TOP_Y = 0; //amount of pixels from the top of the cam to skip
31+
public static int BORDER_BOTTOM_Y = 0; //amount of pixels from the bottom of the cam to skip
32+
33+
//y is fot the outpiut
34+
public static Point LEFT = new Point(50, 120);
35+
public static Point CENTER = new Point(160, 120);
36+
public static Point RIGHT = new Point(270, 120);
37+
38+
public static int VARIANCE = 50;
39+
public static double MIN_AREA = 500;
40+
41+
42+
}
43+
44+
public Exception debug;
45+
46+
47+
private int loopcounter = 0;
48+
private int ploopcounter = 0;
49+
50+
private final Mat mat = new Mat();
51+
private final Mat processed = new Mat();
52+
53+
private Rect maxRect = new Rect();
54+
55+
private double maxArea = 0;
56+
private boolean first = false;
57+
58+
public Telemetry telemetry;
59+
60+
public BarcodePipeline(Telemetry t){
61+
telemetry = t;
62+
}
63+
public BarcodePipeline(){
64+
}
65+
66+
@Override
67+
public Mat processFrame(Mat input)
68+
{
69+
Mat output = input.clone();
70+
try
71+
{
72+
// Process Image
73+
Imgproc.cvtColor(input, mat, Imgproc.COLOR_RGB2RGBA);
74+
Core.inRange(mat, LOWER_LIMIT, UPPER_LIMIT, processed);
75+
// Core.bitwise_and(input, input, output, processed);
76+
77+
// Remove Noise
78+
Imgproc.morphologyEx(processed, processed, Imgproc.MORPH_OPEN, new Mat());
79+
Imgproc.morphologyEx(processed, processed, Imgproc.MORPH_CLOSE, new Mat());
80+
// GaussianBlur
81+
Imgproc.GaussianBlur(processed, processed, new Size(5.0, 15.0), 0.00);
82+
// Find Contours
83+
List<MatOfPoint> contours = new ArrayList<>();
84+
Imgproc.findContours(processed, contours, new Mat(), Imgproc.RETR_LIST, Imgproc.CHAIN_APPROX_SIMPLE);
85+
86+
// Draw Contours
87+
if(DISPLAY) Imgproc.drawContours(output, contours, -1, DISPLAY_COLOR);
88+
89+
// Loop Through Contours
90+
for (MatOfPoint contour : contours)
91+
{
92+
Point[] contourArray = contour.toArray();
93+
94+
// Bound Rectangle if Contour is Large Enough
95+
if (contourArray.length >= 15)
96+
{
97+
MatOfPoint2f areaPoints = new MatOfPoint2f(contourArray);
98+
Rect rect = Imgproc.boundingRect(areaPoints);
99+
100+
// if rectangle is larger than previous cycle or if rectangle is not larger than previous 6 cycles > then replace
101+
if (rect.area() > maxArea
102+
&& rect.x > BORDER_LEFT_X && rect.x + rect.width < input.width() - BORDER_RIGHT_X
103+
&& rect.y > BORDER_TOP_Y && rect.y + rect.height < input.height() - BORDER_BOTTOM_Y
104+
|| loopcounter - ploopcounter > 6)
105+
{
106+
maxArea = rect.area();
107+
maxRect = rect;
108+
ploopcounter++;
109+
loopcounter = ploopcounter;
110+
first = true;
111+
}
112+
areaPoints.release();
113+
}
114+
contour.release();
115+
}
116+
mat.release();
117+
processed.release();
118+
if (contours.isEmpty())
119+
{
120+
maxRect = new Rect();
121+
}
122+
if (first && maxRect.area() > MIN_AREA)
123+
{
124+
if(DISPLAY) Imgproc.rectangle(output, maxRect, DISPLAY_COLOR, 2);
125+
}
126+
// Draw Borders
127+
if(DISPLAY) {
128+
Imgproc.rectangle(output, new Rect(BORDER_LEFT_X, BORDER_TOP_Y, input.width() - BORDER_RIGHT_X - BORDER_LEFT_X, input.height() - BORDER_BOTTOM_Y - BORDER_TOP_Y), DISPLAY_COLOR, 2);
129+
Imgproc.circle(output, LEFT, VARIANCE, DISPLAY_COLOR);
130+
Imgproc.circle(output, CENTER, VARIANCE, DISPLAY_COLOR);
131+
Imgproc.circle(output, RIGHT, VARIANCE, DISPLAY_COLOR);
132+
133+
// Display Data
134+
135+
Imgproc.putText(output, "Area: " + getRectArea() + " Midpoint: " + getRectMidpointXY().x + " , " + getRectMidpointXY().y+ " Selection: "+get(), new Point(20, input.height() - 20), Imgproc.FONT_HERSHEY_PLAIN, 0.6, DISPLAY_COLOR, 1);
136+
}
137+
loopcounter++;
138+
} catch (Exception e) {
139+
debug = e;
140+
boolean error = true;
141+
}
142+
if(telemetry != null) {
143+
telemetry.addLine(get().toString());
144+
telemetry.update();
145+
}
146+
147+
return output;
148+
}
149+
public int getRectHeight(){return maxRect.height;}
150+
public int getRectWidth(){ return maxRect.width; }
151+
public int getRectX(){ return maxRect.x; }
152+
public int getRectY(){ return maxRect.y; }
153+
public double getRectMidpointX(){ return getRectX() + (getRectWidth()/2.0); }
154+
public double getRectMidpointY(){ return getRectY() + (getRectHeight()/2.0); }
155+
public Point getRectMidpointXY(){ return new Point(getRectMidpointX(), getRectMidpointY());}
156+
public double getRectArea(){ return maxRect.area(); }
157+
158+
159+
@Override
160+
public Integer get() {
161+
if(getRectArea()<MIN_AREA) return -1;
162+
Point p = getRectMidpointXY();
163+
if(Math.abs(p.x-LEFT.x)<VARIANCE && Math.abs(p.y-LEFT.y) < VARIANCE) return 0;
164+
if(Math.abs(p.x-CENTER.x)<VARIANCE && Math.abs(p.y-CENTER.y) < VARIANCE) return 1;
165+
if(Math.abs(p.x-RIGHT.x)<VARIANCE && Math.abs(p.y-RIGHT.y) < VARIANCE) return 2;
166+
else return -1; }
167+
}

‎TechnoOneCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/CapSubsystem.java

+8-2
Original file line numberDiff line numberDiff line change
@@ -5,9 +5,11 @@
55

66
import java.util.function.Supplier;
77

8+
import static org.firstinspires.ftc.teamcode.subsystems.CapSubsystem.CapConstant.RESTING;
9+
810
public class CapSubsystem implements Subsystem, Supplier<Double> {
911
public static class CapConstant{
10-
public static final double DEADZONE = 0.1;
12+
public static double RESTING = 0.1;
1113
}
1214

1315
public Servo capServo;
@@ -16,10 +18,14 @@ public CapSubsystem(Servo s){
1618
capServo = s;
1719
}
1820

19-
public void goto_position(double pos){
21+
public void setCap(double pos){
2022
capServo.setPosition(pos);
2123
}
2224

25+
public void restCap(){
26+
setCap(RESTING);
27+
}
28+
2329
@Override
2430
public Double get() {
2531
return null;

0 commit comments

Comments
 (0)