Get best of the week

Implementação de um algoritmo para determinar a largura de um objeto usando um rangefinder ultrassônico e codificadores na linguagem RobotC

Dia bom!

Hoje eu gostaria de compartilhar uma implementação do algoritmo para determinar a largura de um objeto usando um rangefinder ultrassônico e codificadores na linguagem de programação RobotC (usando a plataforma VEX EDR).

Pré-história


Recentemente, me ofereceram a realização de uma master class sobre solução de problemas interessantes usando a linguagem de programação RobotC. Durante a master class, considerei a tarefa de determinar a largura de um objeto, cuja implementação do algoritmo desejo compartilhar nesta publicação.

Base elementar


Como campo de treinamento experimental, usei o ambiente Robot Virtual Worlds para a plataforma VEX, o modelo de robô virtual VEX Launcher, modelos virtuais de elementos de competição VRC Turning Point (Figura 1) e as fórmulas matemáticas mais simples (para determinar o comprimento de um acorde em um círculo com um ângulo central conhecido).

imagem
Figura 1 . Campo de treinamento virtual com um robô

Tarefa


- Determinar o diâmetro da bola usando apenas um telêmetro ultrassônico e codificadores montados no robô

Base teórica


Para determinar a largura do objeto, usei o conceito de padrão de radiação (Figura 2).

imagem
Figura 2. Sensibilidade e padrão direcional do telêmetro ultrassônico

Em essência, foi necessário determinar o ângulo formado ao girar da posição extrema direita para a extrema esquerda entre as tangentes nos pontos extremos do diagrama.
Considere a Figura 3.

imagem
Figura 3 . Geometria do algoritmo

Procuraremos o ângulo central do arco com base na relação entre o raio do ângulo e o arco de rotação do robô. O valor final da largura corresponderá ao acorde m do arco.

Parâmetros de entrada


Como parâmetros de entrada, selecionei:

  • dist = 100 mm (a distância na qual o robô para para medir, o parâmetro é selecionado com base na presença de outros objetos ao redor do objeto que podem cair no campo de visão do sensor e interferir na medição)
  • d_wheel = 102 mm (diâmetro da roda do robô)
  • d_ball = 102 mm (diâmetro real da bola)

Um erro foi determinado como o parâmetro de saída desta tarefa, que indicará a precisão do nosso método e os parâmetros selecionados:

  • erro = abs (d_ball_exper - d_ball)

Algoritmo de Operação do Programa


Os seguintes estágios do algoritmo foram identificados.

  1. Movimento para o objeto.
  2. Gire para o ponto mais à direita de um objeto.
  3. Gire para o ponto mais à esquerda do objeto.
  4. Cálculo do ângulo usando leituras extraídas de codificadores.
  5. Cálculo de erro.

Implementação do algoritmo RobotC


Primeiro de tudo, precisamos inicializar a configuração padrão do robô virtual:

#pragma config(StandardModel, "VEX Launchbot")

Implementamos uma função que será responsável por girar o robô para a extrema direita e / ou extrema esquerda. A função retorna o codificador após uma volta, que será usada em outros cálculos:

int turn_to_over_object(int leftSpeed, int rightSpeed, int distance, bool need_object, bool left_side)
{
if(need_object == true)
{
if(left_side == true)
{
while(SensorValue[sonarSensor] > distance)
{
setMultipleMotors(-leftSpeed, leftMotor,frontLeftMotor);
setMultipleMotors(rightSpeed, rightMotor,frontRightMotor);
}
}
else
{
while(SensorValue[sonarSensor] > distance)
{
setMultipleMotors(leftSpeed, leftMotor,frontLeftMotor);
setMultipleMotors(-rightSpeed, rightMotor,frontRightMotor);
}
}
}
else
{
if(left_side == true)
{
while(SensorValue[sonarSensor] < (distance + 1))
{
setMultipleMotors(-leftSpeed, leftMotor,frontLeftMotor);
setMultipleMotors(rightSpeed, rightMotor,frontRightMotor);
}
}
else
{
while(SensorValue[sonarSensor] < (distance + 1))
{
setMultipleMotors(leftSpeed, leftMotor,frontLeftMotor);
setMultipleMotors(-rightSpeed, rightMotor,frontRightMotor);
}
}
}

return getMotorEncoder(leftMotor);
}

Vamos dar uma olhada nos argumentos da função:

  • leftSpeed ​​- velocidade da unidade esquerda
  • rightSpeed ​​- velocidade certa
  • rightSpeed ​​- velocidade certa
  • distance - distância do objeto em que o robô parou
  • need_object - mode mode: True, se um objeto for necessário no campo de visão do sensor, False, se um objeto for necessário
  • left_side - direção de rotação do robô. Verdadeiro - vire à esquerda, Falso - vire à direita

Com o valor need_object = True, o robô girará até que o objeto esteja a uma distância do robô (no nosso caso, 100 mm).

Com o valor need_object = False, o robô girará até que o objeto esteja a uma distância (distância + 1) do robô. O parâmetro unitário no total afeta a precisão final de determinar a ausência de um objeto no campo de visão do sensor (é possível melhorar o programa levando esse parâmetro a argumentos de função).

Vamos escrever o corpo principal do programa:

task main()
{
int d_ball = 10.2; //sm
int d_wheel = 10.2;

int dist = 10; //sm
//  
setMotor(armMotor, -50);
wait(1);

while(SensorValue[sonarSensor] > 10)
{
setMultipleMotors(50, leftMotor, rightMotor, frontLeftMotor, frontRightMotor);
}
stopAllMotors();
int enc = 0;
enc = turn_to_over_object(50, 50, dist, false, false);
enc = turn_to_over_object(50, 50, dist, true, true);
resetMotorEncoder(leftMotor);
enc = turn_to_over_object(50, 50, dist, false, true);
stopAllMotors();

float teta = abs(enc) * PI * d_wheel * d_wheel / (dist * 627);
float d_ball_exper = 2 * dist * sin(teta / 2);
	
float error = abs(d_ball_exper - d_ball);
while(1)
{
}
}

O primeiro loop while é executado até o robô atingir a distância especificada para o objeto. Em seguida, precisamos chamar a função escrita acima uma a uma para medir a leitura do codificador entre as posições mais à direita e mais à esquerda (observe que antes de medir é necessário redefinir o codificador para zero).

Para obter a fórmula do ângulo, fui guiado pelas seguintes fórmulas:
- enc = N * 627, onde
    N é o número de rotações da roda,
    enc é a leitura do encoder,
    627 é a resolução do encoder => express N;
- S = N * l, onde
    l é o comprimento da roda, l = PI * d_wheel * d_wheel => substituímos N e l => S = enc * PI * d_wheel * d_wheel / 627;
- por outro lado, S = dist * teta, onde
    S é a magnitude do arco que o robô girou,
    teta é o ângulo central desejado;
- com base nos dados acima, expressamos teta => teta = enc * PI * d_wheel * d_wheel / (627 * dist)

Para encontrar o acorde igual ao diâmetro desejado da bola, usamos a fórmula:
    d_ball_exper = 2 * dist * sin (teta / 2)

O loop while sem fim no final do programa é necessário para ver os valores recebidos no depurador.

Dados resumidos


Os seguintes valores do diâmetro e erros da bola foram obtidos no campo virtual:

    d_ball_exper = 10,6 cm
    erro = 0,4 cm

Como você pode ver, o método obtido para medir a largura fornece um pequeno valor de erro, aceitável para a classe de precisão dos sensores utilizados. O valor do erro pode ser reduzido selecionando o valor do parâmetro dist (por exemplo, iniciando um ciclo com o robô retornando com uma determinada etapa que muda para dist) ou usando o controlador PID, que reduzirá a influência da inércia do robô no valor final do codificador durante a frenagem.

Perspectivas


No futuro, considerarei outros métodos de uso de sensores de robô para determinar parâmetros ambientais.

More articles:


All Articles