Mirando com Visual Servoing

1. Você pode mirar seu robô de forma precisa e rápida usando apenas uma limelight e seu sistema de tração.
2. Tudo isso pode ser realizado em menos de 1 hora.

Usando rastreamento de visão de alta taxa de quadros, agora é possível usar o pipeline de visão diretamente como o "sensor" em um loop de controle PID para guiar seu robô ou torre. Para testar essa ideia, adicionamos uma limelight ao nosso robô FRC de 2017 e fizemos ele mirar em alvos de visão usando nada mais que o sistema de tração e os dados da tabela de rede sendo reportados pela limelight.

Neste exemplo, nosso candidato de teste foi um robô FRC de 2017 que usa um sistema de tração de 6 rodas com rodas colson. Aqui está uma foto de nós adicionando uma limelight no robô para fazer este teste.

Em seguida, adicionamos algum código ao robô que seria executado sempre que o piloto segurasse um botão no joystick. Este robô usava direção estilo "tank", então a função OperatorControl estava gerando um valor 'left_command' e um valor 'right_command' para controlar os lados esquerdo e direito do sistema de tração. Após o código de controle normal, adicionamos um bloco de código assim:

float Kp = -0.1f;  // Constante de controle 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;
}

Logo de cara, isso funcionou na maior parte. O robô gira na direção do alvo automaticamente sempre que você está segurando o botão. Se você mover o alvo, o robô gira para seguir o alvo. No entanto, usando o feed de vídeo ao vivo no dashboard, pudemos ver que havia um grande problema: O robô nem sempre conseguia alinhar perfeitamente com o alvo. Em alguns jogos com alvos pequenos (como 2016 e 2017), isso não seria bom o suficiente.

O que implementamos até agora é um simples loop de controle proporcional. Calculamos o erro de direção e multiplicamos por uma constante, assim criando um comando de motor que é proporcional ao erro. Conforme o erro vai a zero, nosso comando vai a zero. O problema é que há muita fricção envolvida quando o robô tenta girar. Comandos muito pequenos não vão girar o robô de forma alguma. Em ângulos pequenos, o comando pode se tornar muito pequeno para realmente mover o robô. Você pode descobrir que seu robô alcança seu alvo bem quando você começa com um grande erro de mira, mas simplesmente não consegue mirar se você começar muito perto.

Existem algumas maneiras de resolver este problema, mas aqui está uma solução realmente simples. Usamos um conceito de "comando mínimo". Se o erro for maior que algum limite, apenas adicione uma constante ao seu comando de motor que representa aproximadamente a quantidade mínima de potência necessária para o robô realmente se mover (na verdade, você quer usar um pouco menos que isso). O novo código fica assim:

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, se você definir Kp ou min_command muito alto, seu robô pode se tornar instável e pode oscilar para frente e para trás conforme ultrapassa o alvo:

Após algum ajuste em Kp e min_command, você deve conseguir que seu robô mire diretamente no alvo de forma muito precisa e rápida.