TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RobotNavigator.java
/**
     * Haifeng's Math function to calculate the new position and heading
     * @param leftEncoder
     * @param rightEncoder
     * @param horEncoder
     */
Haifeng's Math function to calculate the new position and heading
@param leftEncoder
@param rightEncoder
@param horEncoder
Haifeng ' s Math function to calculate the new position and heading @param leftEncoder @param rightEncoder @param horEncoder
public void updateEncoderPos(int leftEncoder, int rightEncoder, int horEncoder) {
        double turnAngle;
        double innerRadius;
        double turnRadius;  
        double xDelta, yDelta;  
        double rightDelta = convertEncoderCountsToCentimeters(rightEncoder - previousRE);
        double leftDelta = convertEncoderCountsToCentimeters(leftEncoder - previousLE);
        double horizDelta = convertEncoderCountsToCentimeters(horEncoder - previousHE);
        if (leftDelta != rightDelta) {
            double leftRadius = profile.hardwareSpec.leftRightWheelDist*leftDelta/(leftDelta-rightDelta);
            turnRadius = leftRadius - profile.hardwareSpec.leftRightWheelDist/2;
            if (leftDelta==0) {
                turnAngle = -rightDelta/profile.hardwareSpec.leftRightWheelDist;
            }
            else {
                turnAngle = leftDelta / leftRadius;
            }
            xDelta = turnRadius*(1-Math.cos(turnAngle)) + horizDelta*Math.cos(turnAngle);
            yDelta = turnRadius*Math.sin(turnAngle)-horizDelta*Math.sin(turnAngle);
            double xTemp = yDelta * Math.sin(heading) + xDelta * Math.cos(heading);
            double yTemp = yDelta * Math.cos(heading) - xDelta * Math.sin(heading);
            xDelta = xTemp;
            yDelta = yTemp;
        }
        else {
            xDelta = leftDelta * Math.sin(heading) + horizDelta * Math.cos(heading);
            yDelta = leftDelta * Math.cos(heading) + horizDelta * Math.sin(heading);
            turnAngle = 0.0;
        }
        worldX += xDelta;
        worldY += yDelta;
        heading += turnAngle;
        previousLE = leftEncoder;
        previousRE = rightEncoder;
        previousHE = horEncoder;
    }
public void updateEncoderPos ( int leftEncoder , int rightEncoder , int horEncoder ) { double turnAngle ; double innerRadius ; double turnRadius ; double xDelta , yDelta ; double rightDelta = convertEncoderCountsToCentimeters ( rightEncoder - previousRE ) ; double leftDelta = convertEncoderCountsToCentimeters ( leftEncoder - previousLE ) ; double horizDelta = convertEncoderCountsToCentimeters ( horEncoder - previousHE ) ; if ( leftDelta != rightDelta ) { double leftRadius = profile . hardwareSpec . leftRightWheelDist * leftDelta / ( leftDelta - rightDelta ) ; turnRadius = leftRadius - profile . hardwareSpec . leftRightWheelDist / 2 ; if ( leftDelta == 0 ) { turnAngle = - rightDelta / profile . hardwareSpec . leftRightWheelDist ; } else { turnAngle = leftDelta / leftRadius ; } xDelta = turnRadius * ( 1 - Math . cos ( turnAngle ) ) + horizDelta * Math . cos ( turnAngle ) ; yDelta = turnRadius * Math . sin ( turnAngle ) - horizDelta * Math . sin ( turnAngle ) ; double xTemp = yDelta * Math . sin ( heading ) + xDelta * Math . cos ( heading ) ; double yTemp = yDelta * Math . cos ( heading ) - xDelta * Math . sin ( heading ) ; xDelta = xTemp ; yDelta = yTemp ; } else { xDelta = leftDelta * Math . sin ( heading ) + horizDelta * Math . cos ( heading ) ; yDelta = leftDelta * Math . cos ( heading ) + horizDelta * Math . sin ( heading ) ; turnAngle = 0.0 ; } worldX += xDelta ; worldY += yDelta ; heading += turnAngle ; previousLE = leftEncoder ; previousRE = rightEncoder ; previousHE = horEncoder ; }
Haifeng's Math function to calculate the new position and heading
 @param leftEncoder
 @param rightEncoder
 @param horEncoder
Haifeng ' s Math function to calculate the new position and heading @param leftEncoder @param rightEncoder @param horEncoder
// consider the center point between left/right track wheel // heading based coordinate position change // Now need to translate the heading coordinate into actual coordinate.