TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PIDMecanumMoveTask.java
/**
     * Multiple criteria to determine completion
     * 1. 0.5cm from target
     * 2. Overshoot (target dist go further from previous measure)
     * 3. Travel distance greater than planned distance
     * 4. No movement for last 40 measures
     * @return
     */
Multiple criteria to determine completion
1.
Multiple criteria to determine completion 1 .
public boolean isDone(){
        double toTargetDist = Math.hypot(endPos.getY()-navigator.getWorldY(), endPos.getX()-navigator.getWorldX());
        if (toTargetDist < 0.5) {
            return true;
        }
        if ((toTargetDist>lastToTargetDist+0.5) && (lastToTargetDist<5)) {
            Logger.logFile("Overshoot - toTarget:" + toTargetDist + " prev:" + lastToTargetDist);
            return true;
        }
        else {
            lastToTargetDist = toTargetDist;
        }
        double targetTravelDistance = Math.hypot(endPos.getY() - startPos.getY(), endPos.getX() - startPos.getX());
        double currentTravelDistance = Math.hypot(navigator.getWorldY() - startPos.getY(), navigator.getWorldX() - startPos.getX());
        boolean noMovement = false;
        if (Math.abs(currentTravelDistance-prevDist[prevNdx])<1) {
            Logger.logFile("MoveBlocked - " + currentTravelDistance + " : " + prevDist[prevNdx]);
            noMovement = true;
        }
        prevDist[prevNdx] = currentTravelDistance;
        prevNdx = (prevNdx + 1) % prevDist.length;
        return (currentTravelDistance > targetTravelDistance) || noMovement;
    }
public boolean isDone ( ) { double toTargetDist = Math . hypot ( endPos . getY ( ) - navigator . getWorldY ( ) , endPos . getX ( ) - navigator . getWorldX ( ) ) ; if ( toTargetDist < 0.5 ) { return true ; } if ( ( toTargetDist > lastToTargetDist + 0.5 ) && ( lastToTargetDist < 5 ) ) { Logger . logFile ( "Overshoot - toTarget:" + toTargetDist + " prev:" + lastToTargetDist ) ; return true ; } else { lastToTargetDist = toTargetDist ; } double targetTravelDistance = Math . hypot ( endPos . getY ( ) - startPos . getY ( ) , endPos . getX ( ) - startPos . getX ( ) ) ; double currentTravelDistance = Math . hypot ( navigator . getWorldY ( ) - startPos . getY ( ) , navigator . getWorldX ( ) - startPos . getX ( ) ) ; boolean noMovement = false ; if ( Math . abs ( currentTravelDistance - prevDist [ prevNdx ] ) < 1 ) { Logger . logFile ( "MoveBlocked - " + currentTravelDistance + " : " + prevDist [ prevNdx ] ) ; noMovement = true ; } prevDist [ prevNdx ] = currentTravelDistance ; prevNdx = ( prevNdx + 1 ) % prevDist . length ; return ( currentTravelDistance > targetTravelDistance ) || noMovement ; }
Multiple criteria to determine completion
 1.
Multiple criteria to determine completion 1 .
//        Logger.logFile("currentDistance: " + currentDistance + ", targetDistance: " + targetDistance); //        Logger.logFile("navigator.getWorldY()=" + navigator.getWorldY() + " navigator.getWorldX() " + navigator.getWorldX()); //        Logger.logFile("noMovement = " + noMovement);