Code Highlights - Pattonville-Robotics/2866 GitHub Wiki

The Vuforia target-seeking method:

while (linearOpMode.opModeIsActive()) {                                                                                                                            
    vuforiaNav.getNearestBeaconLocation();                                                                                                                         
    location = vuforiaNav.getLocation();                                                                                                                           
    locationTelemetry.setValue(Arrays.toString(location));                                                                                                         
                                                                                                                                                                   
    double distanceToBeacon = location[1] / VuforiaNav.MM_PER_INCH, distanceFromCenter = location[0] / VuforiaNav.MM_PER_INCH;                                     
                                                                                                                                                                   
    if (FastMath.abs(distanceToBeacon) < 4)                                                                                                                        
        break;                                                                                                                                                     
                                                                                                                                                                   
    Orientation orientation = Orientation.getOrientation(vuforiaNav.getLastLocation(), AxesReference.EXTRINSIC, AxesOrder.XYZ, AngleUnit.DEGREES);                 
                                                                                                                                                                   
    double correction = (-distanceFromCenter / 50) + (orientation.thirdAngle / 90);                                                                                
    Log.e("ANGLE", "Angle: " + orientation.thirdAngle);                                                                                                            
                                                                                                                                                                   
    double leftPower = approachSpeed + correction;                                                                                                                 
    double rightPower = approachSpeed - correction;                                                                                                                
                                                                                                                                                                   
    powerTelemetry.setValue("Left: " + leftPower + " Right: " + rightPower);                                                                                       
                                                                                                                                                                   
    drive.moveFreely(leftPower, rightPower);                                                                                                                       
    linearOpMode.telemetry.update();                                                                                                                               
}                                                                                                                                                                  

The Encoder movement method:

public void moveInches(Direction direction, double inches, double power) {                                                                       
    //Move Specified Inches Using Motor Encoders                                                                                                 
                                                                                                                                                 
    int targetPositionLeft;                                                                                                                      
    int targetPositionRight;                                                                                                                     
                                                                                                                                                 
    int startPositionLeft = leftDriveMotor.getCurrentPosition();                                                                                 
    int startPositionRight = rightDriveMotor.getCurrentPosition();                                                                               
                                                                                                                                                 
    int deltaPosition = (int) FastMath.round(inchesToTicks(inches));                                                                             
                                                                                                                                                 
    switch (direction) {                                                                                                                         
        case FORWARD: {                                                                                                                          
            targetPositionLeft = startPositionLeft + deltaPosition;                                                                              
            targetPositionRight = startPositionRight + deltaPosition;                                                                            
            break;                                                                                                                               
        }                                                                                                                                        
        case BACKWARD: {                                                                                                                         
            targetPositionLeft = startPositionLeft - deltaPosition;                                                                              
            targetPositionRight = startPositionRight - deltaPosition;                                                                            
            break;                                                                                                                               
        }                                                                                                                                        
        default:                                                                                                                                 
            throw new IllegalArgumentException("Direction must be Direction.FORWARDS or Direction.BACKWARDS!");                                  
    }                                                                                                                                            
                                                                                                                                                 
    DcMotor.RunMode leftDriveMotorMode = leftDriveMotor.getMode();                                                                               
    DcMotor.RunMode rightDriveMotorMode = rightDriveMotor.getMode();                                                                             
                                                                                                                                                 
    leftDriveMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);                                                                                     
    rightDriveMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);                                                                                    
                                                                                                                                                 
    leftDriveMotor.setTargetPosition(targetPositionLeft);                                                                                        
    rightDriveMotor.setTargetPosition(targetPositionRight);                                                                                      
                                                                                                                                                 
    telemetry("Moving " + inches + " inches at power " + power);                                                                                 
    telemetry("LMotorT: " + targetPositionLeft);                                                                                                 
    telemetry("RMotorT: " + targetPositionRight);                                                                                                
    telemetry("EncoderDelta: " + deltaPosition);                                                                                                 
    Telemetry.Item distance = telemetry("DistanceL: -1 DistanceR: -1");                                                                          
                                                                                                                                                 
    move(Direction.FORWARD, power); // To keep power in [0.0, 1.0]. Encoders control direction                                                   
    while (!reachedTarget(leftDriveMotor.getCurrentPosition(), targetPositionLeft, rightDriveMotor.getCurrentPosition(), targetPositionRight)) { 
        Thread.yield();                                                                                                                          
        if (linearOpMode.isStopRequested())                                                                                                      
            break;                                                                                                                               
        distance.setValue("DistanceL: " + leftDriveMotor.getCurrentPosition() + " DistanceR: " + rightDriveMotor.getCurrentPosition());          
        linearOpMode.telemetry.update();                                                                                                         
    }                                                                                                                                            
    stop();                                                                                                                                      
                                                                                                                                                 
    leftDriveMotor.setMode(leftDriveMotorMode); // Restore the prior mode                                                                        
    rightDriveMotor.setMode(rightDriveMotorMode);                                                                                                
}                                                                                                                                                

The Encoder rotation method:

public void rotateDegrees(Direction direction, double degrees, double speed) {                                                                                                              
    //Move specified degrees using motor encoders                                                                                                                                           
                                                                                                                                                                                            
    int targetPositionLeft;                                                                                                                                                                 
    int targetPositionRight;                                                                                                                                                                
                                                                                                                                                                                            
    int startPositionLeft = leftDriveMotor.getCurrentPosition();                                                                                                                            
    int startPositionRight = rightDriveMotor.getCurrentPosition();                                                                                                                          
                                                                                                                                                                                            
    double inches = degreesToInches(degrees);                                                                                                                                               
    int deltaPosition = (int) FastMath.round(inchesToTicks(inches));                                                                                                                        
                                                                                                                                                                                            
    switch (direction) {                                                                                                                                                                    
        case LEFT: {                                                                                                                                                                        
            targetPositionLeft = startPositionLeft - deltaPosition;                                                                                                                         
            targetPositionRight = startPositionRight + deltaPosition;                                                                                                                       
            break;                                                                                                                                                                          
        }                                                                                                                                                                                   
        case RIGHT: {                                                                                                                                                                       
            targetPositionLeft = startPositionLeft + deltaPosition;                                                                                                                         
            targetPositionRight = startPositionRight - deltaPosition;                                                                                                                       
            break;                                                                                                                                                                          
        }                                                                                                                                                                                   
        default:                                                                                                                                                                            
            throw new IllegalArgumentException("Direction must be Direction.LEFT or Direction.RIGHT!");                                                                                     
    }                                                                                                                                                                                       
                                                                                                                                                                                            
    DcMotor.RunMode leftDriveMotorMode = leftDriveMotor.getMode();                                                                                                                          
    DcMotor.RunMode rightDriveMotorMode = rightDriveMotor.getMode();                                                                                                                        
                                                                                                                                                                                            
    leftDriveMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);                                                                                                                                
    rightDriveMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);                                                                                                                               
                                                                                                                                                                                            
    leftDriveMotor.setTargetPosition(targetPositionLeft);                                                                                                                                   
    rightDriveMotor.setTargetPosition(targetPositionRight);                                                                                                                                 
                                                                                                                                                                                            
    Telemetry.Item[] items = new Telemetry.Item[]{                                                                                                                                          
            telemetry("Rotating " + degrees + " degrees at speed " + speed).setRetained(true),                                                                                              
            telemetry("LMotorT: " + targetPositionLeft).setRetained(true),                                                                                                                  
            telemetry("RMotorT: " + targetPositionRight).setRetained(true),                                                                                                                 
            telemetry("EncoderDelta: " + deltaPosition).setRetained(true),                                                                                                                  
            telemetry("DistanceL: DistanceR:")                                                                                                                                              
    };                                                                                                                                                                                      
    Telemetry.Item distance = items[4];                                                                                                                                                     
                                                                                                                                                                                            
    move(Direction.FORWARD, speed); // To keep speed in [0.0, 1.0]. Encoders control direction                                                                                              
    while (!reachedTarget(leftDriveMotor.getCurrentPosition(), targetPositionLeft, rightDriveMotor.getCurrentPosition(), targetPositionRight)) {                                            
        Thread.yield();                                                                                                                                                                     
        if (linearOpMode.isStopRequested())                                                                                                                                                 
            break;                                                                                                                                                                          
        distance.setValue("DistanceL: " + leftDriveMotor.getCurrentPosition() + " DistanceR: " + rightDriveMotor.getCurrentPosition());                                                     
        linearOpMode.telemetry.update();                                                                                                                                                    
    }                                                                                                                                                                                       
    stop();                                                                                                                                                                                 
                                                                                                                                                                                            
    leftDriveMotor.setMode(leftDriveMotorMode); // Restore the prior mode                                                                                                                   
    rightDriveMotor.setMode(rightDriveMotorMode);                                                                                                                                           
                                                                                                                                                                                            
    for (Telemetry.Item i : items)                                                                                                                                                          
        i.setRetained(false);