Holonomic drive OpMode

[dislocated-su]

Driving at any angle

To be able to drive at any angle in all possible directions we had to reassign our input to the stick, as it allowed greater control over the vector we want to head in and helpfully provides it's data as x and y components, thanks to the FTC library.

float x = -gamepad1.left_stick_x;
float y = -gamepad1.left_stick_y;

frontLeftPower = y + -x;
frontRightPower = y + x;
backLeftPower = y + x;
backRightPower = y + -x;

Since the components are split for us, all we have to do is add them to drive in the direction of the vector provided. One issue that arises when using this method is the difference in magnitudes of vectors when driving in certain directions. This was solved previosly while doing the trigonometry method as some trig values gave us higher speeds than others, so we repurposed that code.

double max = Math.abs(findMax(frontLeftPower, frontRightPower, backLeftPower, backRightPower));

if (max != 0) {
    frontLeftPower = (frontLeftPower / max) * basePower;
    frontRightPower = (frontRightPower / max) * basePower;
    backLeftPower = (backLeftPower / max) * basePower;
    backRightPower = (backRightPower / max) * basePower;
}