Apuntando Con Servovisión

1. Puedes apuntar tu robot de manera precisa y rápida usando solo un limelight y tu tren de transmisión.
2. Todo esto se puede lograr en menos de 1 hora.

Usando seguimiento de visión de alta velocidad de cuadros, ahora es posible usar el pipeline de visión directamente como el "sensor" en un bucle de control PID para guiar tu robot o torreta. Para probar esta idea, agregamos un limelight a nuestro robot FRC 2017 e hicimos que apuntara a objetivos de visión usando nada más que el tren de transmisión y los datos de la tabla de red reportados por el limelight.

En este ejemplo, nuestro candidato de prueba fue un robot FRC 2017 que usa un tren de transmisión de 6 ruedas con ruedas colson. Aquí hay una foto de nosotros agregando un limelight al robot para hacer esta prueba.

Luego agregamos algo de código al robot que se ejecutaría cada vez que el conductor mantiene presionado un botón en el joystick. Este robot usaba conducción estilo "tank" así que la función OperatorControl estaba generando un valor 'left_command' y un valor 'right_command' para controlar los lados izquierdo y derecho del tren de transmisión. Después del código de control normal, agregamos un bloque de código como este:

float Kp = -0.1f;  // Constante de control proporcional

std::shared_ptr<NetworkTable> table = NetworkTable::GetTable("limelight");
float tx = table->GetNumber("tx");

if (joystick->GetRawButton(9))
{
	float heading_error = tx;
    steering_adjust = Kp * tx;
    
	left_command+=steering_adjust;
    right_command-=steering_adjust;
}

De entrada, esto funcionó en su mayoría. El robot gira en la dirección del objetivo automáticamente cada vez que mantienes presionado el botón. Si mueves el objetivo, el robot gira para seguirlo. Sin embargo, usando la transmisión de video en vivo en el dashboard, pudimos ver que había un gran problema: El robot no siempre llegaba a alinearse perfectamente con el objetivo. En algunos juegos con objetivos pequeños (como 2016 y 2017) esto no sería suficiente.

Lo que hemos implementado hasta ahora es un simple bucle de control proporcional. Calculamos el error en la orientación y lo multiplicamos por una constante, creando así un comando de motor que es proporcional al error. A medida que el error se acerca a cero, nuestro comando se acercará a cero. El problema es que hay mucha fricción involucrada cuando el robot intenta girar. Comandos muy pequeños no girarán el robot en absoluto. En ángulos pequeños, el comando puede volverse demasiado pequeño para realmente mover el robot. Podrías encontrar que tu robot alcanza su objetivo bien cuando comienzas con un gran error de apuntado pero simplemente no puede apuntar si comienzas muy cerca.

Hay varias formas de resolver este problema pero aquí hay una solución realmente simple. Usamos un concepto de "comando mínimo". Si el error es mayor que algún umbral, simplemente agrega una constante a tu comando de motor que representa aproximadamente la cantidad mínima de potencia necesaria para que el robot realmente se mueva (en realidad quieres usar un poco menos que esto). El nuevo código se ve así:

float Kp = -0.1f;
float min_command = 0.05f;

std::shared_ptr<NetworkTable> table = NetworkTable::GetTable("limelight");
float tx = table->GetNumber("tx");

if (joystick->GetRawButton(9))
{
	float heading_error = -tx;
	float steering_adjust = 0.0f;
	if (Math.abs(heading_error) > 1.0) 
	{
		if (heading_error < 0) 
		{
			steering_adjust = Kp*heading_error + min_command;
		} 
		else 
		{
			steering_adjust = Kp*heading_error - min_command;
		}
	} 
	left_command += steering_adjust;
	right_command -= steering_adjust;
}

Cuidado, si configuras Kp o min_command demasiado alto, tu robot puede volverse inestable y puede oscilar de un lado a otro mientras sobrepasa el objetivo:

Después de algo de ajuste en Kp y min_command deberías lograr que tu robot apunte directamente al objetivo de manera muy precisa y rápida.