Vous avez programmé votre localisation, vous avez obtenu votre coefficients, votre robot peut donc se déplacer comme vous le souhaitez et il vous redonnera ses coordonnées X, Y et Cap à chaque instant.
Pour le moment, à part pour vous la raconter, cela ne sert à rien ! L'intérêt de la localisation c'est de représenter le robot à l'instant t sur une carte d'abscisse X et d'ordonnée Y mais aussi et surtout de représenter aussi sur cette carte un second point qui sera votre cible. L'objectif étant de s'y rendre !
Dans le schémas ci dessus, vous avez obtenu par calcul Xr, Yr et AlphaR les coordonnées et cap du robot. Les coordonnées Xc et Yc du point cible seront définis avant le calcul. Il nous reste deux données que nous devons calculer :
- DirecCibl : L'angle ou cap à prendre
- DistCibl : La distance séparant le robot de la cible
Le cap à prendre :
DirecCibl représente le cap à prendre. C'est à dire que lorsque la diffèrence entre AlphaR et DirecCibl est nulle, le robot est sur la bonne voie ! Il va donc falloir calculer DirecCibl, rien de bien sorcier puisque nos premiers cours de trigonométrie nous donnent :
tan (DirecCibl) = (Yc - Yr) / (Xc - Xr)
donc
(7) DirecCibl = Arctan ( (Yc - Yr) / (Xc- Xr) )
La programmation de ce calcul est assez complexe (pour l'avoir tenté ...) si vous n'utilisez que les blocs du programme de base mais une fois encore c'est Hitechnic et son bloc Atan2 qui nous facilitera la tâche ! Il suffira de calculer les diffèrences de position entre la cible et le robot, de les entrer dans le bloc pour en récupérer la valeur de DirecCibl !
Le bloc atan2
La distance jusqu'à la cible :
La distance calculée ne doit pas servir à alimenter directement un bloc moteur qui avancera d'autant que la distance calculée. Cette valeur devra être utilisée pour définir l'alimentation des moteurs au travers de trois phases : l'accélération, le régime stationnaire et surtout la phase d'approche de la cible. Mais nous n'en sommes pas encore là.
Une fois encore ce sont nos cours de trigonométrie qui vont nous permettre de calculer DistCibl, plus particulièrement grâce au théorème de Pythagore :
(DistCibl)² = (Yc - Yr)² + (Xc - Xr)²
Donc
(8) DistCibl = RACINE CARREE ( (Yc - Yr)² + (Xc - Xr)² )
Malheureusement je n'ai pas trouvé de bloc Racine carrée cette fois-ci, il va donc falloir faire un calcul qui s'en approche. La méthode de Héron d'Alexandrie est une méthode d'approximation de la racine carrée :
(9) rn+1 = (rn + N / rn) / 2
La méthode consiste à 'boucler' plusieurs fois le calcul si dessus avec :
- N qui est le nombre dont on souhaite la racine carrée N = ( (Yc - Yr)² + (Xc - Xr)² )
- rn+1 qui est le résultat le plus proche de la racine carrée rn+1 = DistCibl
- rn qui était le résultat le plus proche lors de la boucle précédente
Pour ceux qui ne comprendraient pas, voici un exemple avec la racine carrée de 12 par exemple. Le résultat est 3,46410 ...
Dans ce cas, N = 12 et nous commencerons toujours avec r0 = 1.
* Donc à n = 0 nous obtenons r1 = (1 + 12 / 1) / 2 = 6,5
* Ensuite à n = 1 nous aurons r2 = (6,5 + 12 / 6,5) / 2 = 4,17307...
* Et encore à n = 2 nous aurons r3 = (4,17307 + 12 / 4,17307) / 2 = 3,5243...
* Enfin à n = 3 nous aurons r3 = (3,5243 + 12 / 3,5243) / 2 = 3,46461...
Il faudra faire autant de boucle que l'on souhaite s'approcher du résultat.
Dans notre cas, N = (Yc - Yr) ² + (Xc - Xr)² et nous commencerons avec r0 = 1, le dernier rn+1 sera DistCibl.
Cependant nous ne savons pas combien de boucle nous devons faire : trop et nous perdrons de la ressource, trop peu et le résultat ne sera pas juste.
Aussi il faut faire le calcul manuellement jusqu'à voir clairement quelle précision on obtient à chaque boucle. Je vous épargne ces calculs, voici mes résultats :
- Pour X = 3000 mm et Y = 2000 mm, les dimensions maxi de la table, il faudra faire 13 boucles pour être à 2% du résultat
- Pour X = 5 mm et Y = 5 mm, il faudra faire seulement 5 boucles pour être juste à quelques centièmes de millimètre.
La réponse se situe en réalité entre les deux et dépendra de notre besoin : A partir de quelle distance avons nous besoin d'être précis ? Personnellement, je pense que c'est à partir du moment où le robot doit décélérer que nous avons besoin d'avoir une information fiable. Pour mon cas, 800 mm, il faudra 11 boucles.
Ma boucle Racine Carrée
Aucun commentaire:
Enregistrer un commentaire