TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoDriveShootTask.java
/**
     * when in picture mode: recognize ring position using OpenCV and translate into field coordinate
     * merge with existing ring array
     */
when in picture mode: recognize ring position using OpenCV and translate into field coordinate
merge with existing ring array
when in picture mode : recognize ring position using OpenCV and translate into field coordinate merge with existing ring array
@Override
    public void execute() {
        if(taskMode == TaskMode.MORE_PIC || taskMode == TaskMode.FIRST_PIC) {
            ArrayList<Rect> ringRecArray = robotHardware.getRobotVision().getRings(saveImg);
            Logger.logFile("Robot see " + ringRecArray.size() + " rings");
            if (ringRecArray.size()!=0) {
                Mat transformed = new Mat();
                ArrayList<Point> imgPosList = new ArrayList<Point>();
                for (Rect r : ringRecArray) {
                    Logger.logFile("Ring Image at " + (r.x + r.width / 2) + ", " + (r.y + r.height / 2));
                    imgPosList.add(new Point(r.x + r.width / 2, r.y + r.height / 2));
                }
                Core.perspectiveTransform(Converters.vector_Point2f_to_Mat(imgPosList), transformed, trans);
                ArrayList<Point> transPoints = new ArrayList<Point>();
                Converters.Mat_to_vector_Point2f(transformed, transPoints);
                for (Point p : transPoints) {
                    Log.d("Point", p.toString());
                    Logger.logFile("Ring robot coordinate at (cm) " + p.x + ", " + p.y);
                }
                for (Point p : transPoints) {
                    Vector2d worldCorr = getFieldCoordinate(robotHardware.getTrackingWheelLocalizer().getPoseEstimate(), p);
                    Logger.logFile("Ring field coordinate at (inch) " + worldCorr.getX() + ", " + worldCorr.getY());
                    int n = 0;
                    while (n < rings.size()) {
                        Vector2d r = rings.get(n);
                        if (r.distTo(worldCorr) < 3) {
                            rings.remove(n);
                        }
                        else {
                            n++;
                        }
                    }
                    rings.add(worldCorr);
                }
            }
        }
        else {
            drive.update();
            if (!drive.isBusy()) {
                if (arrayTraj.size()>0) {
                    Trajectory traj = arrayTraj.remove(0);
                    drive.followTrajectoryAsync(traj);
                }
                else {
                    doneDriving = true;
                }
            }
        }
    }
@ Override public void execute ( ) { if ( taskMode == TaskMode . MORE_PIC || taskMode == TaskMode . FIRST_PIC ) { ArrayList < Rect > ringRecArray = robotHardware . getRobotVision ( ) . getRings ( saveImg ) ; Logger . logFile ( "Robot see " + ringRecArray . size ( ) + " rings" ) ; if ( ringRecArray . size ( ) != 0 ) { Mat transformed = new Mat ( ) ; ArrayList < Point > imgPosList = new ArrayList < Point > ( ) ; for ( Rect r : ringRecArray ) { Logger . logFile ( "Ring Image at " + ( r . x + r . width / 2 ) + ", " + ( r . y + r . height / 2 ) ) ; imgPosList . add ( new Point ( r . x + r . width / 2 , r . y + r . height / 2 ) ) ; } Core . perspectiveTransform ( Converters . vector_Point2f_to_Mat ( imgPosList ) , transformed , trans ) ; ArrayList < Point > transPoints = new ArrayList < Point > ( ) ; Converters . Mat_to_vector_Point2f ( transformed , transPoints ) ; for ( Point p : transPoints ) { Log . d ( "Point" , p . toString ( ) ) ; Logger . logFile ( "Ring robot coordinate at (cm) " + p . x + ", " + p . y ) ; } for ( Point p : transPoints ) { Vector2d worldCorr = getFieldCoordinate ( robotHardware . getTrackingWheelLocalizer ( ) . getPoseEstimate ( ) , p ) ; Logger . logFile ( "Ring field coordinate at (inch) " + worldCorr . getX ( ) + ", " + worldCorr . getY ( ) ) ; int n = 0 ; while ( n < rings . size ( ) ) { Vector2d r = rings . get ( n ) ; if ( r . distTo ( worldCorr ) < 3 ) { rings . remove ( n ) ; } else { n ++ ; } } rings . add ( worldCorr ) ; } } } else { drive . update ( ) ; if ( ! drive . isBusy ( ) ) { if ( arrayTraj . size ( ) > 0 ) { Trajectory traj = arrayTraj . remove ( 0 ) ; drive . followTrajectoryAsync ( traj ) ; } else { doneDriving = true ; } } } }
when in picture mode: recognize ring position using OpenCV and translate into field coordinate
 merge with existing ring array
when in picture mode : recognize ring position using OpenCV and translate into field coordinate merge with existing ring array
// now let's translate from image pixel to robot coordinate // translate to field coordinate and merge to array //when two rings are less than 3 inches apart, assume it's the same ring //DRIVE mode trigger Roadrunner movement