👆🏼 👨🏿‍🚒 😷 Implémentation d'un algorithme de détermination de la largeur d'un objet à l'aide d'un télémètre à ultrasons et d'encodeurs en langage RobotC 🌝 👨🏾‍🎨 🌫️

Bonne journée!

Aujourd'hui, je voudrais partager la mise en œuvre de l'algorithme pour déterminer la largeur d'un objet à l'aide d'un télémètre à ultrasons et d'encodeurs dans le langage de programmation RobotC (en utilisant la plate-forme VEX EDR).

Préhistoire

Récemment, on m'a proposé de diriger une classe de maître sur la résolution de problèmes intéressants en utilisant le langage de programmation RobotC. Lors de la master class, j'ai envisagé la tâche de déterminer la largeur de l'objet, dont l'implémentation de l'algorithme que je souhaite partager dans cette publication.

Base élémentaire

Comme terrain d'entraînement expérimental, j'ai utilisé l'environnement Robot Virtual Worlds pour la plate-forme VEX, le modèle de robot virtuel VEX Launcher, des modèles virtuels d'éléments de compétition VRC Turning Point (figure 1) et les formules mathématiques les plus simples (pour déterminer la longueur d'un accord dans un cercle avec un angle central connu).

Graphique 1 . Terrain d'entraînement virtuel avec un robot

Tâche

- Déterminer le diamètre de la balle en utilisant uniquement un télémètre à ultrasons et des encodeurs montés sur le robot

Base théorique

Pour déterminer la largeur de l'objet, j'ai utilisé le concept d'un diagramme de rayonnement (figure 2).

Figure 2. Sensibilité et motif directionnel du télémètre à ultrasons

En substance, il était nécessaire de déterminer l'angle formé lors du passage de la position extrême droite à la position extrême gauche entre les tangentes aux points extrêmes du diagramme.
Considérez la figure 3.

Figure 3 . Géométrie de l'algorithme

Nous rechercherons l' angle central de l'arc en fonction de la relation entre le rayon de l'angle et l'arc de rotation du robot. La valeur de largeur finale correspondra à la corde m de l'arc.

Paramètres d'entrée

En tant que paramètres d'entrée, j'ai sélectionné:

dist = 100 mm (la distance à laquelle le robot s'arrête pour mesurer, le paramètre est sélectionné en fonction de la présence d'autres objets autour de l'objet qui peuvent tomber dans le champ de vision du capteur et interférer avec la mesure)
d_wheel = 102 mm (diamètre de la roue du robot)
d_ball = 102 mm (diamètre réel de la balle)

Une erreur a été déterminée comme paramètre de sortie de cette tâche, ce qui indiquera la précision de notre méthode et les paramètres sélectionnés:

erreur = abs (d_ball_exper - d_ball)

Algorithme de fonctionnement du programme

Les étapes suivantes de l'algorithme ont été identifiées.

Déplacement vers l'objet.
Pivoter à l'extrême droite d'un objet.
Faire pivoter vers le point extrême gauche de l'objet.
Calcul de l'angle à l'aide de relevés issus de codeurs.
Calcul d'erreur.

Implémentation RobotC de l'algorithme

Tout d'abord, nous devons initialiser la configuration standard du robot virtuel:

#pragma config(StandardModel, "VEX Launchbot")

Nous implémentons une fonction qui sera chargée de faire tourner le robot vers l'extrême droite et / ou l'extrême gauche. La fonction renvoie l'encodeur après un tour, qui sera utilisé dans d'autres calculs:

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);
}

Examinons de plus près les arguments de la fonction:

leftSpeed - vitesse d'entraînement gauche
rightSpeed - bonne vitesse d'entraînement
rightSpeed - bonne vitesse d'entraînement
distance - distance à l'objet où le robot s'est arrêté
need_object - mode mouvement: Vrai, si un objet est nécessaire dans le champ de vision du capteur, Faux si un objet est nécessaire
left_side - sens de rotation du robot. Vrai - tourner à gauche, Faux - tourner à droite

Avec la valeur need_object = True, le robot tournera jusqu'à ce que l'objet soit à distance du robot (dans notre cas, 100 mm).

Avec la valeur need_object = False, le robot tournera jusqu'à ce que l'objet soit à une distance (distance + 1) du robot. Le paramètre unitaire au total affecte la précision finale de détermination de l'absence d'un objet dans le champ de vision du capteur (il est possible d'améliorer le programme en intégrant ce paramètre dans les arguments de fonction).

Écrivons le corps principal du programme:

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)
{
}
}

La première boucle while s'exécute jusqu'à ce que le robot atteigne la distance spécifiée à l'objet. Ensuite, nous devons appeler la fonction écrite ci-dessus une par une afin de mesurer la lecture du codeur entre les positions la plus à droite et la plus à gauche (notez qu'avant de mesurer, il est nécessaire de remettre le codeur à zéro).

Pour obtenir la formule d'angle, j'ai été guidé par les formules suivantes:
- enc = N * 627, où
    N est le nombre de tours de la roue,
    enc est la lecture du codeur,
    627 est la résolution du codeur => N express;
- S = N * l, où
    l est la longueur de la roue, l = PI * d_wheel * d_wheel => on remplace N et l => S = enc * PI * d_wheel * d_wheel / 627;
- d'autre part, S = dist * teta, où
    S est la grandeur de l'arc que le robot a tourné,
    teta est l'angle central souhaité;
- sur la base des données ci-dessus, nous exprimons teta => teta = enc * PI * d_wheel * d_wheel / (627 * dist)

Pour trouver l'accord égal au diamètre souhaité de la balle, nous utilisons la formule:
    d_ball_exper = 2 * dist * sin (teta / 2)

La boucle while sans fin à la fin du programme est nécessaire pour voir les valeurs reçues dans le débogueur.

Données récapitulatives

Les valeurs suivantes du diamètre de la balle et des erreurs ont été obtenues sur le champ virtuel:

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

Comme vous pouvez le voir, la méthode obtenue pour mesurer la largeur donne une petite valeur d'erreur, ce qui est acceptable pour la classe de précision des capteurs utilisés. La valeur d'erreur peut être réduite en sélectionnant la valeur du paramètre dist (par exemple, en commençant un cycle avec le robot reculant avec une certaine étape qui passe à dist) ou en utilisant le contrôleur PID, ce qui réduira l'influence de l'inertie du robot sur la valeur finale de l'encodeur pendant le freinage.

Perspectives

À l'avenir, j'examinerai d'autres méthodes d'utilisation de capteurs robotiques pour déterminer les paramètres environnementaux.

Implémentation d'un algorithme de détermination de la largeur d'un objet à l'aide d'un télémètre à ultrasons et d'encodeurs en langage RobotC