Test Swerve Drive Code

MyClass.java

// Use this to test:
// https://www.jdoodle.com/online-java-compiler-ide/

public class MyClass {
    public static void main(String args[]) {
      
        // Manual test cases that we found to be problematic
        printTest(0.0f, 0.0f, 0.0f);
        
        printTest(90.0f, 270.0f, 90.0f);
        
        printTest(10.0f, 350.0f, -10.0f);
       
        printTest(350.0f, 10.0f, 370.0f);
        
        printTest((350.0f + (3.0f * 360.0f)), 10.0f, (370.0f + (3.0f * 360.0f)));
        
        printTest((350.0f + (-3.0f * 360.0f)), 10.0f, (370.0f + (-3.0f * 360.0f)));
        
        printTest((10.0f + (3.0f * 360.0f)), 350.0f, (-10.0f + (3.0f * 360.0f)));
        
        printTest((10.0f + (-1.0f * 360.0f)), 350.0f, (-10.0f + (-1.0f * 360.0f)));
        
        printTest(90.0f, 350.0f, 170.0f);
        
        printTest(350.0f, 90.0f, 270.0f);
        
        printTest(54.0f, 0.0f, 0.0f);
        
        
        // Automatic test cases testing each quadrant of position and targets
        double[] targetOffsets = { -10.0f, 10.0f, 90.0f, 180.0f, 270.0f };

        double[] positions = { 0.0f, 45.0f, 90.0f, 135.0f, 180.0f, 225.0f, 270.0f, 315.0f, 360.0f };


        for(int i=0; i < positions.length; i++){
            double[] targets = generateTargetsForPosition(positions[i], targetOffsets);
        
            checkPositionsWithTargets(positions[i], targets, targetOffsets);
        }
    }
    
    public static double[] generateTargetsForPosition(double position, double[] targetOffsets){
        double[] targets = new double[5];
        
        for(int i=0; i < targets.length; i++){
            targets[i] = (position + targetOffsets[i]) % 360;
        }
    
        return targets;
        
    }
    
    public static void checkPositionsWithTargets(double position, double[] targets, double[] targetOffsets){
        // 1st quadrant
        printTest(position, targets[0], position + targetOffsets[0]);
        printTest(position, targets[1], position + targetOffsets[1]);
        
        // 2nd quadrant
        printTest(position, targets[2], position + targetOffsets[2]);
        
        // 3rd quadrant
        printTest(position, targets[3], position);
        
        // 4th quadrant
        printTest(position, targets[4], position - targetOffsets[2]);
    }
    
    public static void printTest(double position, double target, double goal){
        SwerveTarget t = closestAngle(position, target);
        
        if(t.getTarget() != goal){
            System.out.println("-----------------------------------------");
            System.out.println("Position:\tTarget:");
            System.out.println(position + "     \t" + target);
            System.out.println("");
          
            System.out.println("Goal:   \tTarget:   \tScale:");
            System.out.println(goal + "     \t" + t.getTarget() + "     \t" + t.getMotorScale());
    
            System.out.println("-----------------------------------------");
            System.out.println("");
        }
    }
    
    public static SwerveTarget closestAngle(double p, double t){
        double pTemp = p;
        
        p %= 360;
        if(p < 0) p += 360;
        
        double t1 = t % 360;
        
        double d1 = t1 - p;
        if(d1 > 180) d1 = d1 - 360;
        if(d1 < -180) d1 = d1 + 360;

        double d2 = (d1 > 0 ? d1 - 180 : 180 + d1);
        double df = (Math.abs(d1) <= Math.abs(d2) ? d1 : d2);
        double motorScale = (Math.abs(d1) <= Math.abs(d2) ? 1 : -1);

        double target = pTemp + df;

        return new SwerveTarget(target, motorScale);
    }
}

SwerveTarget.java

public class SwerveTarget {
    private double target;
    private double motorScale;

    public SwerveTarget(double target, double motorScale){
        this.target = target;
        this.motorScale = motorScale;
    }

    public double getTarget(){
        return this.target;
    }

    public double getMotorScale(){
        return this.motorScale;
    }
}