2866-Decode/RevisedWheelControls.java at main · Pattonville-Robotics/2866-Decode · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
package org.firstinspires.ftc.teamcode;

public class RevisedWheelControls {
    private double[] multipliers;
    private RefitSquare refitSquare;

    public RevisedWheelControls()
    {
        multipliers = new double[] {1,1,1,1};
        refitSquare = new RefitSquare();
    }

    public RevisedWheelControls(double[] input_multipliers)
    {
        multipliers = input_multipliers;
        refitSquare = new RefitSquare();
    }

    public RevisedWheelControls(double frontLeft, double frontRight, double backLeft, double backRight)
    {
        multipliers = new double[] {frontLeft, frontRight, backLeft, backRight};
        refitSquare = new RefitSquare();
    }

    private double[] rotateAngle(double angle, double[] pos)
    {
        double f = pos[0]*Math.cos(angle)-pos[1]*Math.sin(angle);
        double b = pos[0]*Math.sin(angle)+pos[1]*Math.cos(angle);
        double[] fb = {f,b};
        return(fb);
    }

    public double[] disentangle_uncorrected(double[] pos)
    {
        return(rotateAngle(-45*(Math.PI/180),new double[]{pos[0],pos[1]}));
    }

    public double[] disentangle_corrected(double[] pos)
    {
        return(refitSquare.convertPoint(disentangle_uncorrected(pos)));
    }

    public double[] calculate(double x1, double y1, double x2, double y2)
    {
        double[] leftJoystick = new double[] {x1,y1};
        double[] rightJoystick = new double[] {x2,y2};

        double[] leftWheels = disentangle_corrected(leftJoystick);
        double[] rightWheels = disentangle_corrected(rightJoystick);

        double frontLeft=leftWheels[1];
        double frontRight=leftWheels[0];
        double backLeft=rightWheels[0];
        double backRight=rightWheels[1];

        frontLeft*=multipliers[0];
        frontRight*=multipliers[1];
        backLeft*=multipliers[2];
        backRight*=multipliers[3];

        double[] return_list = new double[] {frontLeft,frontRight,backLeft,backRight};
        return(return_list);
    }

    public double[] calculate(double[] values)
    {
        return(calculate(values[0],values[1],values[2],values[3]));
    }

    public double[] get_multipliers()
    {
        return(multipliers);
    }
}

//Willow Harpole, 1/18/2026