FtcRobotController/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/FirstPipelineRevised.java at master · dragonher0/FtcRobotController · GitHub

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
package org.firstinspires.ftc.teamcode;


import org.opencv.core.Core;
import org.opencv.core.Mat;

import org.opencv.core.Rect;
import org.opencv.core.Scalar;
import org.opencv.imgproc.Imgproc;
import android.graphics.Canvas;
import org.firstinspires.ftc.vision.VisionProcessor;
import org.firstinspires.ftc.robotcore.internal.camera.calibration.CameraCalibration;


public class FirstPipelineRevised implements VisionProcessor
{
    public Scalar nonSelectedColor = new Scalar(0,255,0);
    public Scalar selectedColor = new Scalar(0,0,255);
    public Rect rectLeft = new Rect(50, 450, 80, 80);
    public Rect rectMiddle = new Rect(600, 450, 80, 80);
    public Rect rectRight = new Rect(1100, 450, 80, 80);
    public int selectedRect = 0;
    Selected select;
    double selected = 0;

    Mat hsvMat = new Mat();
    Mat destMat = new Mat();
    Mat detectionMat = new Mat();
    Mat submat = new Mat();
    @Override
    public void init(int width, int height, CameraCalibration calibration) {

    }
    @Override
    public Object processFrame(Mat frame, long captureTimeNanos)
    {
        Imgproc.cvtColor(frame, hsvMat, Imgproc.COLOR_RGB2HSV);
        double satLeft = getAvgSaturation(hsvMat, rectLeft);
        double satMiddle = getAvgSaturation(hsvMat, rectMiddle);
        double satRight = getAvgSaturation(hsvMat, rectRight);
        Core.extractChannel(hsvMat, detectionMat, 1);
        Imgproc.cvtColor(detectionMat, destMat, Imgproc.COLOR_GRAY2RGB);

        if ((satLeft > satMiddle) && (satLeft > satRight)) {
            selected = 1;
            select = Selected.LEFT;
        } else if ((satMiddle > satLeft) && (satMiddle > satRight)) {
            selected = 2;
            select = Selected.MIDDLE;
        } else {
            selected = 3;
            select = Selected.RIGHT;
        }


        // REMOVE DURING PRODUCTION
        drawRectangles(frame, selected);
        // END REMOVE DURING PRODUCTION
        return select;
    }
    protected double getAvgSaturation(Mat input, Rect rect) {
        submat = input.submat(rect);
        Scalar color = Core.mean(submat);
        return color.val[1];
    }

    public void drawRectangles(Mat input, double selected)
    {
        Imgproc.rectangle(input, rectLeft, nonSelectedColor);
        Imgproc.rectangle(input, rectMiddle, nonSelectedColor);
        Imgproc.rectangle(input, rectRight, nonSelectedColor);

        if (selected == 1) {
            Imgproc.rectangle(input, rectLeft, selectedColor);
        } else if (selected == 2) {
            Imgproc.rectangle(input, rectMiddle, selectedColor);
        } else if (selected == 3) {
            Imgproc.rectangle(input, rectRight, selectedColor);
        }
    }
    @Override
    public void onDrawFrame(Canvas canvas, int onscreenWidth, int onscreenHeight, float scaleBmpPxToCanvasPx, float scaleCanvasDensity, Object userContext) {

    }
    public enum Selected {
        NONE,
        LEFT,
        MIDDLE,
        RIGHT
    }
    public double getSelection() {
        return selected;
    }
}