FTC esporádicos codificador de valores (3 / 4 paso)

Paso 3: Solucionar el problema: código

Esta es la función de movimiento que utilizamos en autónoma. Toma una entrada de distancia (calculado pulgadas para facilitar la programación), la velocidad, una dirección de destino y una opción de freno/costa. Para que funcione, una partida de activo se requiere, así como una dirección de destino.
El funcionamiento de esto combina los valores actuales de codificador para obtener un funcionamiento total como las unidades de robot. Las funciones leftMotor() y rightMotor() Obten alimentados a un número y todos los motores de coche a esa velocidad. Podría sustituirse por "motor [motorLeft] = (velocidad + corrección)" y "motor [motorRight] = (velocidad - corrección)".

void moveDistanceAtSpeedOnHeading (float distancia, velocidad de int, float targetHeading, corto goNoGo)
{
diff de flotador;
Float calculatedDistance;
Float combinedEncoderDistance;
corrección del int;
calculatedDistance = abs(distance * 120);
diff = targetHeading - partida;
nMotorEncoder [motorLeft] = 0;
nMotorEncoder [motorRight] = 0;
wait1Msec(200);
for(;;)
{
combinedEncoderDistance = abs (nMotorEncoder [motorLeft] + nMotorEncoder[motorRight]);
if(combinedEncoderDistance > calculatedDistance)
{
mientras que (combinedEncoderDistance == abs (nMotorEncoder [motorLeft] + nMotorEncoder[motorRight]))
{
wait1Msec(5);
}
Si (abs (nMotorEncoder [motorLeft] + nMotorEncoder[motorRight]) > calculatedDistance)
{
continueLoop = false;
rotura;
}
}
diff = targetHeading - partida;
corrección = diff * CORRECTIONGAIN;
leftMotor (velocidad + corrección);
rightMotor (velocidad - corrección);
wait1Msec(5); añadido un valor de 5, ya que se trata de dos veces tan rápido como los codificadores para actualizar sus valores
}
if(goNoGo == NO_GO)
{
leftMotor(0);
rightMotor(0);
}
}