TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoVisionPickup.java
// Enumerate all chosen ring possible pick up order and get quickest one
Enumerate all chosen ring possible pick up order and get quickest one
Enumerate all chosen ring possible pick up order and get quickest one
ArrayList<Trajectory> generatePath(ArrayList<Vector2d> rings) {
        ArrayList<Vector2d> chosen = selectRings(rings);
        Logger.logFile("Selected rings - " + chosen.size());
        ArrayList<Trajectory> bestMove = new ArrayList<Trajectory>();
        int moveExamined = 0;
        long startTime = System.currentTimeMillis();
        double bestDuration = 10000;
        for (int a = 0; a < chosen.size(); a++) {
            for (int b = 0; b < chosen.size(); b++) {
                if (a != b) {
                    for (int c = 0; c < chosen.size(); c++) {
                        if (a != b && a != c && b != c) {
                            double currDuration = 0;
                            moveExamined++;
                            ArrayList<Trajectory> move = pickUpAndShoot(chosen.get(a), chosen.get(b), chosen.get(c));
                            for(Trajectory t : move) {
                                currDuration = currDuration + t.duration();
                            }
                            if (bestDuration > currDuration) {
                                bestDuration = currDuration;
                                bestMove = move;
                            }
                        }
                    }
                }
            }
        }
        long timeSpent = System.currentTimeMillis() - startTime;
        Logger.logFile("Example " + moveExamined + " time: " + timeSpent + " avg: " + 1.0*timeSpent/moveExamined);
        return bestMove;
    }
ArrayList < Trajectory > generatePath ( ArrayList < Vector2d > rings ) { ArrayList < Vector2d > chosen = selectRings ( rings ) ; Logger . logFile ( "Selected rings - " + chosen . size ( ) ) ; ArrayList < Trajectory > bestMove = new ArrayList < Trajectory > ( ) ; int moveExamined = 0 ; long startTime = System . currentTimeMillis ( ) ; double bestDuration = 10000 ; for ( int a = 0 ; a < chosen . size ( ) ; a ++ ) { for ( int b = 0 ; b < chosen . size ( ) ; b ++ ) { if ( a != b ) { for ( int c = 0 ; c < chosen . size ( ) ; c ++ ) { if ( a != b && a != c && b != c ) { double currDuration = 0 ; moveExamined ++ ; ArrayList < Trajectory > move = pickUpAndShoot ( chosen . get ( a ) , chosen . get ( b ) , chosen . get ( c ) ) ; for ( Trajectory t : move ) { currDuration = currDuration + t . duration ( ) ; } if ( bestDuration > currDuration ) { bestDuration = currDuration ; bestMove = move ; } } } } } } long timeSpent = System . currentTimeMillis ( ) - startTime ; Logger . logFile ( "Example " + moveExamined + " time: " + timeSpent + " avg: " + 1.0 * timeSpent / moveExamined ) ; return bestMove ; }
Enumerate all chosen ring possible pick up order and get quickest one
Enumerate all chosen ring possible pick up order and get quickest one
// build a path for different pick up orders