public void setWheelPowers(double power1, double power2, double power3, double power4) {
// TODO: USE PID CONTROL
_swervePod1.setDriveMotor(power1);
_swervePod2.setDriveMotor(power2);
_swervePod3.setDriveMotor(power3);
_swervePod4.setDriveMotor(power4);
}
public void setAllWheelPowers(double power) { // Set all wheel pods to the same power
// TODO: USE PID CONTROL
_swervePod1.setDriveMotor(power);
_swervePod2.setDriveMotor(power);
_swervePod3.setDriveMotor(power);
_swervePod4.setDriveMotor(power);
}
public void setCrab(double podAngles, double powerLeft, double powerRight) {
setAllWheelAngles(podAngles);
// TODO: Set drive power with PID
_swervePod1.setDriveMotor(powerLeft); // Left
_swervePod4.setDriveMotor(powerLeft); // Left
_swervePod2.setDriveMotor(powerRight); // Right
_swervePod3.setDriveMotor(powerRight); // Right
}