TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RobotNavigator.java
//William Gutrich 10/13/19, refactor code to a method to set all positions
William Gutrich 10/13/19, refactor code to a method to set all positions
William Gutrich 10 / 13 / 19 refactor code to a method to set all positions
public  void updateAllPositionsOld(int leftEncoder, int rightEncoder, int horEncoder) {
        double x=0,y = 0;
        double angle = 0;
        double tempY=0, tempX=0;
        deltaHE = convertEncoderCountsToCentimeters(horEncoder - previousHE);
        deltaLE = convertEncoderCountsToCentimeters(leftEncoder - previousLE);
        deltaRE = convertEncoderCountsToCentimeters(rightEncoder - previousRE);
        double abDiffEncoder = Math.abs(deltaLE) - Math.abs(deltaRE);
        if (deltaLE != deltaRE) {
            double radius = profile.hardwareSpec.leftRightWheelDist * deltaLE / (deltaLE - deltaRE);
            if(Math.abs(radius)> profile.hardwareSpec.leftRightWheelDist / 2) {
                center_radius = radius - profile.hardwareSpec.leftRightWheelDist / 2;
            }else{
                center_radius = radius;
            }
            if (deltaLE == 0) {
                angle = -deltaRE / profile.hardwareSpec.leftRightWheelDist;
            }else if(center_radius == 0){
                angle =  Math.asin((deltaLE-deltaRE)/radius);
            }else {
                angle =  deltaLE /  center_radius;
            }
            double y1 = center_radius * Math.sin(angle);
            double x1 = center_radius * (1-Math.cos(angle));
            double x2 = deltaHE * Math.cos(angle);
            double y2 = deltaHE * Math.sin(angle);
            x = x1 + x2 ;
            y = y1 - y2;
            tempX = y * Math.sin(heading) + x * Math.cos(heading);
            tempY = y * Math.cos(heading) - x * Math.sin(heading);
        } else {
            tempX = deltaLE * Math.sin(heading) + deltaHE * Math.cos(heading) ;
            tempY = deltaLE * Math.cos(heading) + deltaHE * Math.sin(heading);
        }
        heading = angleWrap( heading + angle);
        worldY  += tempY;
        worldX  += tempX;
        setEncoderCounts(leftEncoder,rightEncoder,horEncoder);
    }
public void updateAllPositionsOld ( int leftEncoder , int rightEncoder , int horEncoder ) { double x = 0 , y = 0 ; double angle = 0 ; double tempY = 0 , tempX = 0 ; deltaHE = convertEncoderCountsToCentimeters ( horEncoder - previousHE ) ; deltaLE = convertEncoderCountsToCentimeters ( leftEncoder - previousLE ) ; deltaRE = convertEncoderCountsToCentimeters ( rightEncoder - previousRE ) ; double abDiffEncoder = Math . abs ( deltaLE ) - Math . abs ( deltaRE ) ; if ( deltaLE != deltaRE ) { double radius = profile . hardwareSpec . leftRightWheelDist * deltaLE / ( deltaLE - deltaRE ) ; if ( Math . abs ( radius ) > profile . hardwareSpec . leftRightWheelDist / 2 ) { center_radius = radius - profile . hardwareSpec . leftRightWheelDist / 2 ; } else { center_radius = radius ; } if ( deltaLE == 0 ) { angle = - deltaRE / profile . hardwareSpec . leftRightWheelDist ; } else if ( center_radius == 0 ) { angle = Math . asin ( ( deltaLE - deltaRE ) / radius ) ; } else { angle = deltaLE / center_radius ; } double y1 = center_radius * Math . sin ( angle ) ; double x1 = center_radius * ( 1 - Math . cos ( angle ) ) ; double x2 = deltaHE * Math . cos ( angle ) ; double y2 = deltaHE * Math . sin ( angle ) ; x = x1 + x2 ; y = y1 - y2 ; tempX = y * Math . sin ( heading ) + x * Math . cos ( heading ) ; tempY = y * Math . cos ( heading ) - x * Math . sin ( heading ) ; } else { tempX = deltaLE * Math . sin ( heading ) + deltaHE * Math . cos ( heading ) ; tempY = deltaLE * Math . cos ( heading ) + deltaHE * Math . sin ( heading ) ; } heading = angleWrap ( heading + angle ) ; worldY += tempY ; worldX += tempX ; setEncoderCounts ( leftEncoder , rightEncoder , horEncoder ) ; }
William Gutrich 10/13/19, refactor code to a method to set all positions
William Gutrich 10 / 13 / 19 refactor code to a method to set all positions
////10/5 Gavin and Hayden, calculate angle in each cycle //        Logger.logFile("deltaHE = " + deltaHE); //        Logger.logFile("deltaLE = "+ deltaLE); //        Logger.logFile("deltaRE = " + deltaRE); //x, y are the displacement in each drive cycle //                Logger.logFile("deltaLE==0, angle = " + angle); //                Logger.logFile("center_radius == 0, angle = " + angle); //                Logger.logFile("angle = " + angle); //apply the local/relative changes to the coordination system - adjust to start from 0 degree //angle = 0; //this is the heading angle relative to starting position //        Logger.logFile("old heading = " + heading); //        Logger.logFile("heading = " + heading); //worldAngle = -Math.toDegrees(Math.atan2(worldY,worldX) - Math.PI/2);