Algorithme du mouvement

Présentation

Cet algorithme est le responsable pour que le serpent puisse marcher en avant et en arrière.

En bref, l'idée ici est de mapper les vertèbres du serpent dans une sinusoïde, en calculant la position de toutes les vertèbres sur cette onde. Ensuite, le calcul admet qu’un petit décalage a été fait (il assume que le serpent va se décaler) et recalcule toutes les positions. Si ce "mapping" est fait en utilisant un très petit "delay" entre les calculs et si les vertèbres respectent ce format d’onde, il est possible d'observer un mouvement du serpent.

Pour calculer les positions et arriver à mapper les vertèbres, nous fixons la première vertèbre (par exemple, tout au début dans la position 0) et calculons les positions des deux prochaines vertèbres. Pour qu’une vertèbre puisse calculer son angle de mouvement, elle a besoin de trois positions. Lorsqu'elles sont trouvées, ces positions sont envoyées à la deuxième vertèbre, qui calcule la position de la quatrième. Ce processus se répète jusqu’à la dernière vertèbre.

Comme le servomoteur qui contrôle les angles verticaux se trouve au bout d’une vertèbre et pas à son milieu (c’est-à-dire qu’une vertèbre n’est pas symétrique), pour faire les calcules pour qu’elle puisse avancer ou reculer, on doit utiliser des positions différentes des vertèbres.

Ainsi, pour le faire avancer, voici les positions nécessaires pour les vertèbres (xY, où Y indique la position de la vertèbre dans le serpent) :

  • 1ère : x1, x2 et x3 ;
  • 2ème : x2, x3, x4 ;
  • 3ème : x3, x4, x5 ;

Et pour le faire reculer, les positions nécessaires sont les suivantes :

  • 10ème : x10, x10, x9 ;
  • 9ème : x10, x9, x8 ;
  • 8ème : x9, x8, x7 ;

Calcul de la position de la prochaine vertèbre

Pour faire le calcul de la position de la prochaine vertèbre, nous partons de deux principes :

  • Les extrémités de chaque vertèbres sont mappées sur une sinusoïde ;
  • La longueur d’une vertèbre est constante (15 cm).

De cette façon, si nous admettons qui le point du début de la vertèbre est (x0,y0) le point (x1,y1) est son autre extrémité, nous avons :

(x1 – x0)^2 + (y1 – y0)^2 = 15^2
(x1 – x0)^2 + (sin(x1) – sin(x0))^2 = 15^2

Si x0 est connu, nous pouvons trouver le point x1 (et conséquemment le point y1) de manière itérative, par approximations.

Pour réaliser ce calcul itérative, nous avons implémenté deux algorithme : de la dichotomie et de Brent (qui implémente les algorithmes de la dichotomie, de la sécante et de l’interpolation quadratique inverse). Celui de la dichotomie est plus assuré, mais il peut mettre beaucoup de temps pour converger. Par contre, celui de Brent converge plus vite, mais il est moins assuré. Quand le calcul n’est pas bon, il revient et utilise la méthode de la dichotomie.

À la fin, nous avons aperçu que la méthode de la dichotomie était suffi pour nous et celle que nous utilisons pour nos calculs.

Exemple

Voici un petit graphique qui montre l’avancement d’un serpent théorique dans une sinusoïde (utilisé pour valider notre code en virgule fixe) :

sinusoide.png

—-
Back

Unless otherwise stated, the content of this page is licensed under Creative Commons Attribution-ShareAlike 3.0 License