PBL Project

Programming Editors

Repl.it:

In order to have an ease of access when sharing, communicating, and distributing code, we use Repl.it to store and hold our code. It is essentially helpful when migrating from different controller hubs as a universal programmer. When sharing code, using the "Java" icon is able to compute the code without any necessary errors. 

We use Repl.it as a specifically safe way to store code online, while also being able to efficiently access it on any device including mobile, laptop, or desktop. By doing this, we are able to 

FTC EDITOR:

The main FTC software is connected to the hubs on the robot, and is used to run, compute, and execute the code we program. This is the main editor, where all the code is directly sent to the Control Hub in order to run and execute any commands. In this software, we have a range of tools such as Auto-Importing, as well as the Auto-Fill text for code lines. 

This software is an efficient source in order to speed up delivery time and increase robot efficiency. 

ANDROID STUDIO:

Android studio is an all-in-one coding editing software. This studio is used to share files as well as execute code on the robot. Being a more heavy and advanced application, the system requirements are greater and larger, making them best suitable to work on laptops and desktops. The process of syncing files is also a little bit more complicated. After figuring out how to navigate the studio in the most efficient manner, the process becomes smoother and easier to work with.  This software also helps us create a high powered and efficient robot game, so small conflicting things such as the phone malfunction doesn't occur. 

INVESTIGATION QUESTIONS: 

1. How do we utilize OnBotJava to create a suitable code for our robot to run and follow?

OnBotJava allows to program our robot in Java code on an easy-to-use platform and it integrates the simplicity of block programs along with complex Android Studio code. The overall user interface for OnBotJava is easy enough to understand although it may take some practice getting used it. We rate this a 8 as it is still a good overall feel and works with the functionality appropriately. The functionality for OnBotJava is an amazing choice as it integrates the simplicity of block programs along with complex Android Studio code. 

2. How do we use OPENCV in OnBotJava?

The overall user interface for OPENCV is easy to understand although it may take some practice getting used to. By pulling libraries from local GitHubs, we were able to optimize our code and our robot. We integrated April Tags, which are tags similar to QR codes and had our robots camera sensor scan it and run a specific program once it was initialized. 

3. How does understanding the design in the robot affect the way we program it?

Understanding the main concept of the robot and how the robot runs is extremely necessary in order to maximize the efficiency and accuracy of our robots movements. By gathering information on how the robot will run and function is important for programmers to understand how they will make the robot move and navigate throughout the playing field. The designing process of the robot goes hand in hand with the basic and complex programming of it.

4. What was the hardest part of programming the robot?

The hardest part of programming our robot was finding out how to efficiently gather the most points in the shortest amount of time in a single match. This was able to be resolved quickly through trial and error and through test runs, we were able to come up with many solutions to scoring many points under 30 seconds. 

PROGRAM CODE:

package org.firstinspires.ftc.teamcode;

import org.opencv.calib3d.Calib3d;

import org.opencv.core.CvType;

import org.opencv.core.Mat;

import org.opencv.core.MatOfDouble;

import org.opencv.core.MatOfPoint2f;

import org.opencv.core.MatOfPoint3f;

import org.opencv.core.Point;

import org.opencv.core.Point3;

import org.opencv.core.Scalar;

import org.opencv.imgproc.Imgproc;

import org.openftc.apriltag.AprilTagDetection;

import org.openftc.apriltag.AprilTagDetectorJNI;

import org.openftc.easyopencv.OpenCvPipeline;

import java.util.ArrayList;

class AprilTagDetectionPipeline extends OpenCvPipeline

{

    private long nativeApriltagPtr;

    private Mat grey = new Mat();

    private ArrayList<AprilTagDetection> detections = new ArrayList<>();

    private ArrayList<AprilTagDetection> detectionsUpdate = new ArrayList<>();

    private final Object detectionsUpdateSync = new Object();

    Mat cameraMatrix;

    Scalar blue = new Scalar(7,197,235,255);

    Scalar red = new Scalar(255,0,0,255);

    Scalar green = new Scalar(0,255,0,255);

    Scalar white = new Scalar(255,255,255,255);

    double fx;

    double fy;

    double cx;

    double cy;

    // UNITS ARE METERS

    double tagsize;

    double tagsizeX;

    double tagsizeY;

    private float decimation;

    private boolean needToSetDecimation;

    private final Object decimationSync = new Object();

    public AprilTagDetectionPipeline(double tagsize, double fx, double fy, double cx, double cy)

    {

        this.tagsize = tagsize;

        this.tagsizeX = tagsize;

        this.tagsizeY = tagsize;

        this.fx = fx;

        this.fy = fy;

        this.cx = cx;

        this.cy = cy;

        constructMatrix();

        // Allocate a native context object. See the corresponding deletion in the finalizer

        nativeApriltagPtr = AprilTagDetectorJNI.createApriltagDetector(AprilTagDetectorJNI.TagFamily.TAG_36h11.string, 3, 3);

    }

    @Override

    public void finalize()

    {

        // Might be null if createApriltagDetector() threw an exception

        if(nativeApriltagPtr != 0)

        {

            // Delete the native context we created in the constructor

            AprilTagDetectorJNI.releaseApriltagDetector(nativeApriltagPtr);

            nativeApriltagPtr = 0;

        }

        else

        {

            System.out.println("AprilTagDetectionPipeline.finalize(): nativeApriltagPtr was NULL");

        }

    }

    @Override

    public Mat processFrame(Mat input)

    {

        // Convert to greyscale

        Imgproc.cvtColor(input, grey, Imgproc.COLOR_RGBA2GRAY);

        synchronized (decimationSync)

        {

            if(needToSetDecimation)

            {

                AprilTagDetectorJNI.setApriltagDetectorDecimation(nativeApriltagPtr, decimation);

                needToSetDecimation = false;

            }

        }

        // Run AprilTag

        detections = AprilTagDetectorJNI.runAprilTagDetectorSimple(nativeApriltagPtr, grey, tagsize, fx, fy, cx, cy);

        synchronized (detectionsUpdateSync)

        {

            detectionsUpdate = detections;

        }

        // For fun, use OpenCV to draw 6DOF markers on the image. We actually recompute the pose using

        // OpenCV because I haven't yet figured out how to re-use AprilTag's pose in OpenCV.

        for(AprilTagDetection detection : detections)

        {

            Pose pose = poseFromTrapezoid(detection.corners, cameraMatrix, tagsizeX, tagsizeY);

            drawAxisMarker(input, tagsizeY/2.0, 6, pose.rvec, pose.tvec, cameraMatrix);

            draw3dCubeMarker(input, tagsizeX, tagsizeX, tagsizeY, 5, pose.rvec, pose.tvec, cameraMatrix);

        }

        return input;

    }

    public void setDecimation(float decimation)

    {

        synchronized (decimationSync)

        {

            this.decimation = decimation;

            needToSetDecimation = true;

        }

    }

    public ArrayList<AprilTagDetection> getLatestDetections()

    {

        return detections;

    }

    public ArrayList<AprilTagDetection> getDetectionsUpdate()

    {

        synchronized (detectionsUpdateSync)

        {

            ArrayList<AprilTagDetection> ret = detectionsUpdate;

            detectionsUpdate = null;

            return ret;

        }

    }

    void constructMatrix()

    {

        //     Construct the camera matrix.

        //

        //      --         --

        //     | fx   0   cx |

        //     | 0    fy  cy |

        //     | 0    0   1  |

        //      --         --

        //

        cameraMatrix = new Mat(3,3, CvType.CV_32FC1);

        cameraMatrix.put(0,0, fx);

        cameraMatrix.put(0,1,0);

        cameraMatrix.put(0,2, cx);

        cameraMatrix.put(1,0,0);

        cameraMatrix.put(1,1,fy);

        cameraMatrix.put(1,2,cy);

        cameraMatrix.put(2, 0, 0);

        cameraMatrix.put(2,1,0);

        cameraMatrix.put(2,2,1);

    }

    /**

     * Draw a 3D axis marker on a detection. (Similar to what Vuforia does)

     *

     * @param buf the RGB buffer on which to draw the marker

     * @param length the length of each of the marker 'poles'

     * @param rvec the rotation vector of the detection

     * @param tvec the translation vector of the detection

     * @param cameraMatrix the camera matrix used when finding the detection

     */

    void drawAxisMarker(Mat buf, double length, int thickness, Mat rvec, Mat tvec, Mat cameraMatrix)

    {

        // The points in 3D space we wish to project onto the 2D image plane.

        // The origin of the coordinate space is assumed to be in the center of the detection.

        MatOfPoint3f axis = new MatOfPoint3f(

                new Point3(0,0,0),

                new Point3(length,0,0),

                new Point3(0,length,0),

                new Point3(0,0,-length)

        );

        // Project those points

        MatOfPoint2f matProjectedPoints = new MatOfPoint2f();

        Calib3d.projectPoints(axis, rvec, tvec, cameraMatrix, new MatOfDouble(), matProjectedPoints);

        Point[] projectedPoints = matProjectedPoints.toArray();

        // Draw the marker!

        Imgproc.line(buf, projectedPoints[0], projectedPoints[1], red, thickness);

        Imgproc.line(buf, projectedPoints[0], projectedPoints[2], green, thickness);

        Imgproc.line(buf, projectedPoints[0], projectedPoints[3], blue, thickness);

        Imgproc.circle(buf, projectedPoints[0], thickness, white, -1);

    }

    void draw3dCubeMarker(Mat buf, double length, double tagWidth, double tagHeight, int thickness, Mat rvec, Mat tvec, Mat cameraMatrix)

    {

        //axis = np.float32([[0,0,0], [0,3,0], [3,3,0], [3,0,0],

        //       [0,0,-3],[0,3,-3],[3,3,-3],[3,0,-3] ])

        // The points in 3D space we wish to project onto the 2D image plane.

        // The origin of the coordinate space is assumed to be in the center of the detection.

        MatOfPoint3f axis = new MatOfPoint3f(

                new Point3(-tagWidth/2, tagHeight/2,0),

                new Point3( tagWidth/2, tagHeight/2,0),

                new Point3( tagWidth/2,-tagHeight/2,0),

                new Point3(-tagWidth/2,-tagHeight/2,0),

                new Point3(-tagWidth/2, tagHeight/2,-length),

                new Point3( tagWidth/2, tagHeight/2,-length),

                new Point3( tagWidth/2,-tagHeight/2,-length),

                new Point3(-tagWidth/2,-tagHeight/2,-length));

        // Project those points

        MatOfPoint2f matProjectedPoints = new MatOfPoint2f();

        Calib3d.projectPoints(axis, rvec, tvec, cameraMatrix, new MatOfDouble(), matProjectedPoints);

        Point[] projectedPoints = matProjectedPoints.toArray();

        // Pillars

        for(int i = 0; i < 4; i++)

        {

            Imgproc.line(buf, projectedPoints[i], projectedPoints[i+4], blue, thickness);

        }

        // Base lines

        //Imgproc.line(buf, projectedPoints[0], projectedPoints[1], blue, thickness);

        //Imgproc.line(buf, projectedPoints[1], projectedPoints[2], blue, thickness);

        //Imgproc.line(buf, projectedPoints[2], projectedPoints[3], blue, thickness);

        //Imgproc.line(buf, projectedPoints[3], projectedPoints[0], blue, thickness);

        // Top lines

        Imgproc.line(buf, projectedPoints[4], projectedPoints[5], green, thickness);

        Imgproc.line(buf, projectedPoints[5], projectedPoints[6], green, thickness);

        Imgproc.line(buf, projectedPoints[6], projectedPoints[7], green, thickness);

        Imgproc.line(buf, projectedPoints[4], projectedPoints[7], green, thickness);

    }

    /**

     * Extracts 6DOF pose from a trapezoid, using a camera intrinsics matrix and the

     * original size of the tag.

     *

     * @param points the points which form the trapezoid

     * @param cameraMatrix the camera intrinsics matrix

     * @param tagsizeX the original width of the tag

     * @param tagsizeY the original height of the tag

     * @return the 6DOF pose of the camera relative to the tag

     */

    Pose poseFromTrapezoid(Point[] points, Mat cameraMatrix, double tagsizeX , double tagsizeY)

    {

        // The actual 2d points of the tag detected in the image

        MatOfPoint2f points2d = new MatOfPoint2f(points);

        // The 3d points of the tag in an 'ideal projection'

        Point3[] arrayPoints3d = new Point3[4];

        arrayPoints3d[0] = new Point3(-tagsizeX/2, tagsizeY/2, 0);

        arrayPoints3d[1] = new Point3(tagsizeX/2, tagsizeY/2, 0);

        arrayPoints3d[2] = new Point3(tagsizeX/2, -tagsizeY/2, 0);

        arrayPoints3d[3] = new Point3(-tagsizeX/2, -tagsizeY/2, 0);

        MatOfPoint3f points3d = new MatOfPoint3f(arrayPoints3d);

        // Using this information, actually solve for pose

        Pose pose = new Pose();

        Calib3d.solvePnP(points3d, points2d, cameraMatrix, new MatOfDouble(), pose.rvec, pose.tvec, false);

        return pose;

    }

    /*

     * A simple container to hold both rotation and translation

     * vectors, which together form a 6DOF pose.

     */

    class Pose

    {

        Mat rvec;

        Mat tvec;

        public Pose()

        {

            rvec = new Mat();

            tvec = new Mat();

        }

        public Pose(Mat rvec, Mat tvec)

        {

            this.rvec = rvec;

            this.tvec = tvec;

        }

    }

}