RaydiumWikiNi

CommandeBras

PagePrincipale :: DerniersChangements :: ParametresUtilisateur :: Vous êtes ec2-54-80-227-189.compute-1.amazonaws.com
Retour

II/ Commande des différents moteurs



PLAN


II.1/ Comment fonctionnent-ils? Comment les commander?

II.2/ Fonctions pour simplifier l'utilisation

Matrice

II.4/ La méthode géométrique








II.1/ Comment fonctionnent ces servomoteurs?


Afin de diriger ces moteurs, il nous faut leur appliquer à chacun une impulsion entre une et deux millisecondes toutes les vingt millisecondes.

Une impulsion d'une milliseconde positionne le servomoteur en butée gauche.
Une impulsion de deux millisecondes positionne le servomoteur en butée droite.
Une impulsion d'une milliseconde et demie positionne le servomoteur au milieu.



Comment commander ces servomoteurs?


Nous utilisons un micro-processeur M32C83.
Nous avons donc deux possibilités pour générer ces impulsions.

L'avantage de la PWM est qu'il ne dépend pas du processeur, et donc il y a moins de risque d'erreur.
Mais nécessite six PWM pour commander les sept moteurs.

L'avantage de l'utilisation d'un timer est que l'on peut rajouter des fonctionnalités, car seulement un timer est utilisé ( contrairement à six dans le cas de la PWM).
Mais il sera moins précis, car utilisant des interruptions, certains problèmes peuvent arriver.



II.2/ Fonctions pour simplifier l'utilisation


Nous avons choisit d'utiliser la deuxième méthode, c'est à dire l'utilisation d'un seul timer.

Pour cela nous devons définir deux conditions:
Les ports que nous avons utilisés pour relier le microcontrôleur et le bras robot sont:







Voici le code que nous avons rédigé pour pouvoir faire ces conditions:


//
#define MILLISEC 6000
#define REPOS 13200
#define COEFF 44 
 


void timer()
{
	//
 
	static int etat=0; //
	static int num=0; //
 
	//
	switch (num)
	{
		case 0 : p2_0 = 1 - etat; break;
		case 1 : p2_1 = 1 - etat; break;
		case 2 : p2_3 = 1 - etat; break;
		case 3 : p2_2 = 1 - etat; break;
		case 4 : p2_4 = 1 - etat; break;
		case 5 : p2_5 = 1 - etat; break;
	}


//
	//
	if ( etat == 0 )
	{
		ta1 = REPOS - ta1;
	}
	//
	//
	else
	{
		switch (num)
		{
			case 0 :  ta1= MILLISEC + COEFF*angA;       // Calcul de la valeur de Ta1 pour le moteur A //
				break; 
			case 1 : 	ta1= MILLISEC + COEFF*angB; // Calcul de la valeur de Ta1 pour le moteur B //
				break;
			case 2 : 	ta1= MILLISEC + COEFF*angC; // Calcul de la valeur de Ta1 pour le moteur C //
				break;
			case 3 : 	ta1= MILLISEC + COEFF*angD; // Calcul de la valeur de Ta1 pour le moteur D //
				break;
			case 4 : 	ta1= MILLISEC + COEFF*angE; // Calcul de la valeur de Ta1 pour le moteur E //
				break;
			case 5 : 	ta1= MILLISEC + COEFF*angF; // Calcul de la valeur de Ta1 pour le moteur F //
				break;
		}
		
		if (num++ == 5)	num=0;
	}
	
	//
	etat = 1 - etat;
}


Maintenant que nous savons comment contrôler les servomoteurs, nous allons voir comment calculer les valeurs des angles à leurs envoyer.
Pour ceci, nous avons le choix entre deux méthodes:
- Les matrices de déplacement
- La méthode géométrique

II.3/ Matrice de déplacement


ICI


II.4/ La méthode géométrique


Afin de positionner la pince grâce aux coordonnées (X1, Y1, Z1), nous sommes passés par un calcul géométrique.

Nous avons décidé de laisser la pince à l'horizontale, ce qui nous permet de calculer que trois angles.

Afin de calculer A nous allons utiliser la tangente:




A = Arc tangente ( X1 / Y1 )

d² = X1² + Y1²

Calcul de d, et de l'angle à appliquer pour le moteur A
if (coorX == 0)
			{
				if (coorY < 18) coorY = 18;
				d = coorY - 13; //
				angA_cons = 0;
			}
		else if (coorY == 0)
			{
				if (coorX > 0)
					{
						if (coorX < 18) coorX = 18;					
						angA_cons = 90;
					}
				else if (coorX < 0)
					{
						if (coorX > -18) coorX = -18;
						angA_cons = -90;
					}
				
				d = abs(coorX) - 13;	
			}
		else if (sqrt( (coorX*coorX) + (coorY*coorY)) > 18)
			{
				d = sqrt( (coorX*coorX) + (coorY*coorY))-13; 
				angA_cons = atan(coorX/coorY)*(180/PI);
			}

Maintenant que nous avons calculé d et A, nous pouvons passer à la suite:



d2² = d² + ( Z1 - 8 ) Z1 - 8 sert à enlever la base pour calculer à partir du point de pivot B

z1 = Arc tangente ( ( Z1 - 8) / d )
z2 = Arc sinus ( racine ( 12² - ( d2 / 2 )² ) / 12 )

B = z1 + z2

C = 2 * Arc sinus ( ( d2 / 2 ) / 12 )
C = 2 * Arc sinus ( d2 / 24 )

Calcul de d2

d2 = sqrt( (d*d) + ((coorZ-8)*(coorZ-8)));


Calcul de l'angle à appliquer pour le moteur B

if (d2 <= 24) // 24 == longueur maximum de d2 (12 + 12) 
			{
				
				z1 = atan((coorZ-8)/d)*(180/PI);
				z2 = asin(sqrt(144 - (d2/2)*(d2/2))/12.0f)*(float) (180.0f/PI);
 
				angB_cons = (z1 + z2);


Calcul de l'angle à appliquer pour le moteur C

angC_cons = -(2*(asin(d2/24.0f)*(float) (180.0f/PI))-90);


Arrivé à ce point, nous pouvons positionner le point de pivot ou l'on veut.
Il nous reste plus qu'à calculer D, l'angle à appliquer au moteur D afin que la pince soit toujours horizontale.





on a :

D + B + C = 180
D = 180 - B - C

Mais vu que sur le robot l'angle moteur C = 0 correspond à un angle de 90°
D = 180 - B + ( 90 - C)


Calcul de l'angle à appliquer pour le moteur D

angD_cons = -(angB_cons +(90-angC_cons)) + 170;
 //
 

Retour