-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathOpenCV_Contour_3954_Test
147 lines (128 loc) · 6.04 KB
/
OpenCV_Contour_3954_Test
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
package org.firstinspires.ftc.teamcode;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
//import org.firstinspires.ftc.teamcode.utility.Globalvalues;
import org.opencv.core.Scalar;
import org.openftc.easyopencv.OpenCvCamera;
import org.openftc.easyopencv.OpenCvCameraFactory;
import org.openftc.easyopencv.OpenCvCameraRotation;
@Config //Disable if not using FTC Dashboard https://github.com/PinkToTheFuture/OpenCV_FreightFrenzy_2021-2022#opencv_freightfrenzy_2021-2022
@Autonomous(name="OpenCV_Contour_3954_Test", group="Tutorials")
public class OpenCV_Contour_3954_Test extends LinearOpMode {
private OpenCvCamera webcam;
private static final int CAMERA_WIDTH = 640; // width of wanted camera resolution
private static final int CAMERA_HEIGHT = 360; // height of wanted camera resolution
private double CrLowerUpdate = 160;
private double CbLowerUpdate = 100;
private double CrUpperUpdate = 255;
private double CbUpperUpdate = 255;
public static double borderLeftX = 0.0; //fraction of pixels from the left side of the cam to skip
public static double borderRightX = 0.0; //fraction of pixels from the right of the cam to skip
public static double borderTopY = 0.0; //fraction of pixels from the top of the cam to skip
public static double borderBottomY = 0.0; //fraction of pixels from the bottom of the cam to skip
private double lowerruntime = 0;
private double upperruntime = 0;
// Pink Range Y Cr Cb
public static Scalar scalarLowerYCrCb = new Scalar( 0.0, 160.0, 100.0);
public static Scalar scalarUpperYCrCb = new Scalar(255.0, 255.0, 255.0);
// Yellow Range
// public static Scalar scalarLowerYCrCb = new Scalar(0.0, 100.0, 0.0);
// public static Scalar scalarUpperYCrCb = new Scalar(255.0, 170.0, 120.0);
@Override
public void runOpMode()
{
// OpenCV webcam
int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName());
webcam = OpenCvCameraFactory.getInstance().createWebcam(hardwareMap.get(WebcamName.class, "Webcam 1"), cameraMonitorViewId);
//OpenCV Pipeline
ContourPipeline myPipeline;
webcam.setPipeline(myPipeline = new ContourPipeline(borderLeftX,borderRightX,borderTopY,borderBottomY));
// Configuration of Pipeline
myPipeline.configureScalarLower(scalarLowerYCrCb.val[0],scalarLowerYCrCb.val[1],scalarLowerYCrCb.val[2]);
myPipeline.configureScalarUpper(scalarUpperYCrCb.val[0],scalarUpperYCrCb.val[1],scalarUpperYCrCb.val[2]);
// Webcam Streaming
webcam.openCameraDeviceAsync(new OpenCvCamera.AsyncCameraOpenListener()
{
@Override
public void onOpened()
{
webcam.startStreaming(CAMERA_WIDTH, CAMERA_HEIGHT, OpenCvCameraRotation.UPRIGHT);
}
@Override
public void onError(int errorCode)
{
/*
* This will be called if the camera could not be opened
*/
}
});
// Only if you are using ftcdashboard
FtcDashboard dashboard = FtcDashboard.getInstance();
telemetry = new MultipleTelemetry(telemetry, dashboard.getTelemetry());
FtcDashboard.getInstance().startCameraStream(webcam, 10);
telemetry.update();
waitForStart();
while (opModeIsActive())
{
myPipeline.configureBorders(borderLeftX, borderRightX, borderTopY, borderBottomY);
if(myPipeline.error){
telemetry.addData("Exception: ", myPipeline.debug);
}
// Only use this line of the code when you want to find the lower and upper values
testing(myPipeline);
telemetry.addData("RectArea: ", myPipeline.getRectArea());
telemetry.update();
if(myPipeline.getRectArea() > 2000){
if(myPipeline.getRectMidpointX() > 400){
AUTONOMOUS_C();
}
else if(myPipeline.getRectMidpointX() > 200){
AUTONOMOUS_B();
}
else {
AUTONOMOUS_A();
}
}
}
}
public void testing(ContourPipeline myPipeline){
if(lowerruntime + 0.05 < getRuntime()){
CrLowerUpdate += -gamepad1.left_stick_y;
CbLowerUpdate += gamepad1.left_stick_x;
lowerruntime = getRuntime();
}
if(upperruntime + 0.05 < getRuntime()){
CrUpperUpdate += -gamepad1.right_stick_y;
CbUpperUpdate += gamepad1.right_stick_x;
upperruntime = getRuntime();
}
CrLowerUpdate = inValues(CrLowerUpdate, 0, 255);
CrUpperUpdate = inValues(CrUpperUpdate, 0, 255);
CbLowerUpdate = inValues(CbLowerUpdate, 0, 255);
CbUpperUpdate = inValues(CbUpperUpdate, 0, 255);
myPipeline.configureScalarLower(0.0, CrLowerUpdate, CbLowerUpdate);
myPipeline.configureScalarUpper(255.0, CrUpperUpdate, CbUpperUpdate);
telemetry.addData("lowerCr ", (int)CrLowerUpdate);
telemetry.addData("lowerCb ", (int)CbLowerUpdate);
telemetry.addData("UpperCr ", (int)CrUpperUpdate);
telemetry.addData("UpperCb ", (int)CbUpperUpdate);
}
public Double inValues(double value, double min, double max){
if(value < min){ value = min; }
if(value > max){ value = max; }
return value;
}
public void AUTONOMOUS_A(){
telemetry.addLine("Autonomous A");
}
public void AUTONOMOUS_B(){
telemetry.addLine("Autonomous B");
}
public void AUTONOMOUS_C(){
telemetry.addLine("Autonomous C");
}
}