¡Buen día!Hoy me gustaría compartir una implementación del algoritmo para determinar el ancho de un objeto usando un telémetro ultrasónico y codificadores en el lenguaje de programación RobotC (usando la plataforma VEX EDR).Prehistoria
Recientemente, me ofrecieron realizar una clase magistral sobre la resolución de problemas interesantes utilizando el lenguaje de programación RobotC. Durante la clase magistral, consideré la tarea de determinar el ancho del objeto, la implementación del algoritmo que quiero compartir en esta publicación.Base elemental
Como campo de entrenamiento experimental, utilicé el entorno Robot Virtual Worlds para la plataforma VEX, el modelo de robot virtual VEX Launcher, modelos virtuales de elementos de competencia VRC Turning Point (Figura 1) y las fórmulas matemáticas más simples (para determinar la longitud de un acorde en un círculo con un ángulo central conocido).
Figura 1 . Campo de entrenamiento virtual con un robot.Tarea
- Determinar el diámetro de la bola usando solo un telémetro ultrasónico y codificadores montados en el robotBases teóricas
Para determinar el ancho del objeto, utilicé el concepto de un patrón de radiación (Figura 2).
Figura 2. Patrón de sensibilidad y radiación del buscador de rango ultrasónicoEsencialmente, fue necesario determinar el ángulo formado al girar desde la posición extrema derecha a la posición extrema izquierda entre las tangentes en los puntos extremos del diagrama.Considere la Figura 3.
Figura 3 . Geometría de algoritmo Buscaremos elángulo central del arco basado en la relación entre el radio del ángulo y el arco de rotación del robot. El valor del ancho final corresponderá al acorde m del arco.Parámetros de entrada
Como parámetros de entrada, he seleccionado:- dist = 100 mm (la distancia a la que el robot se detiene para medir, el parámetro se selecciona en función de la presencia de otros objetos alrededor del objeto que pueden caer en el campo de visión del sensor e interferir con la medición)
- d_wheel = 102 mm (diámetro de la rueda del robot)
- d_ball = 102 mm (diámetro real de la bola)
Se determinó un error como el parámetro de salida de esta tarea, que indicará la precisión de nuestro método y los parámetros seleccionados:- error = abs (d_ball_exper - d_ball)
Algoritmo de Operación del Programa
Se identificaron las siguientes etapas del algoritmo.- Movimiento al objeto.
- Rotar al extremo derecho de un objeto.
- Gire hacia el extremo izquierdo del objeto.
- Cálculo del ángulo utilizando lecturas tomadas de codificadores.
- Cálculo de error.
Implementación RobotC del algoritmo
En primer lugar, necesitamos inicializar la configuración estándar del robot virtual:#pragma config(StandardModel, "VEX Launchbot")
Implementamos una función que será responsable de girar el robot a la extrema derecha y / o extrema izquierda. La función devuelve el codificador después de un giro, que se utilizará en otros 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);
}
Echemos un vistazo más de cerca a los argumentos de la función:- leftSpeed - velocidad de conducción izquierda
- rightSpeed - velocidad de conducción correcta
- rightSpeed - velocidad de conducción correcta
- distancia - distancia al objeto donde se detuvo el robot
- need_object - modo de movimiento: verdadero, si se necesita un objeto en el campo de visión del sensor, falso si se necesita un objeto
- left_side: dirección de rotación del robot. Verdadero - gire a la izquierda, falso - gire a la derecha
Con el valor need_object = True, el robot rotará hasta que el objeto esté a una distancia del robot (en nuestro caso, 100 mm).Con el valor need_object = False, el robot rotará hasta que el objeto esté a una distancia (distancia + 1) del robot. El parámetro de la unidad en total afecta la precisión final de determinar la ausencia de un objeto en el campo de visión del sensor (es posible mejorar el programa tomando este parámetro en los argumentos de la función).Escribamos el cuerpo principal del 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)
{
}
}
El primer ciclo while se ejecuta hasta que el robot alcanza la distancia especificada al objeto. A continuación, debemos llamar a la función escrita una por una para medir las lecturas del codificador entre las posiciones más a la derecha e izquierda (tenga en cuenta que antes de medir es necesario restablecer el codificador a cero).Para obtener la fórmula del ángulo, me guiaron por las siguientes fórmulas:- enc = N * 627, donde N es el número de revoluciones de la rueda, enc es la lectura del codificador, 627 es la resolución del codificador => express N;- S = N * l, donde l es la longitud de la rueda, l = PI * d_wheel * d_wheel => sustituto de N y l => S = enc * PI * d_wheel * d_wheel / 627;- por otro lado, S = dist * teta, donde S es la magnitud del arco que giró el robot, teta es el ángulo central deseado;- basado en los datos anteriores, expresamos teta => teta = enc * PI * d_wheel * d_wheel / (627 * dist)Para encontrar el acorde igual al diámetro deseado de la bola, usamos la fórmula: d_ball_exper = 2 * dist * sin (teta / 2)El bucle while sin fin al final del programa es necesario para ver los valores recibidos en el depurador.Resumen de datos
Se obtuvieron los siguientes valores del diámetro de la pelota y los errores en el campo virtual: d_ball_exper = 10.6 cm error = 0.4 cmComo puede ver, el método obtenido para medir el ancho proporciona un pequeño valor de error, que es aceptable para la clase de precisión de los sensores utilizados. El valor del error se puede reducir seleccionando el valor del parámetro dist (por ejemplo, iniciando un ciclo con el robot retrocediendo con un cierto paso que cambia a dist) o utilizando el controlador PID, lo que reducirá la influencia de la inercia del robot en el valor final del codificador durante el frenado.Perspectivas
En el futuro, consideraré otros métodos de uso de sensores de robot para determinar los parámetros ambientales.