Here is the code from our FTC library.
Code:
/**
* This method implements tank drive where leftPower controls the left motors and right power controls the right
* motors.
*
* @param leftPower specifies left power value.
* @param rightPower specifies right power value.
* @param inverted specifies true to invert control (i.e. robot front becomes robot back).
*/
public void tankDrive(double leftPower, double rightPower, boolean inverted)
{
leftPower = TrcUtil.clipRange(leftPower);
rightPower = TrcUtil.clipRange(rightPower);
if (inverted)
{
double swap = leftPower;
leftPower = -rightPower;
rightPower = -swap;
}
if (gyro != null)
{
//
// Gyro assist is enabled.
//
double drivePower = (leftPower + rightPower)/2.0;
double diffPower = (leftPower - rightPower)/2.0;
double turnPower = gyroAssistKp*(diffPower - gyroRateScale*gyro.getZRotationRate().value);
leftPower = drivePower + turnPower;
rightPower = drivePower - turnPower;
double maxMag = Math.max(Math.abs(leftPower), Math.abs(rightPower));
if (maxMag > 1.0)
{
leftPower /= maxMag;
rightPower /= maxMag;
}
}
if (frontLeftMotor != null)
{
frontLeftMotor.setPower(leftPower);
}
if (frontRightMotor != null)
{
frontRightMotor.setPower(rightPower);
}
if (rearLeftMotor != null)
{
rearLeftMotor.setPower(leftPower);
}
if (rearRightMotor != null)
{
rearRightMotor.setPower(rightPower);
}
} //tankDrive