108 réponses à ce sujet

[Vaelan]

hello

Désole d avoir était aussi long j ai pas eu trop le temps mais j ai fait ça pour ton robot

 

alors pour commencer comme je savais pas comment est monté ton robot je suis partis sur cette base

 

tu pourras corriger si ton robot est monté différemment

 

pour la cinématique elle est sur géogébra parce que je l'ai testé virtuellement avant de te la proposer

désolé c'est un peu petit, mais tu peux agrandir en ouvrant le lien dans une nouvelle fenêtre(je sais pas comment le mettre en plus grand)

 

Avec ça tu as une base qui fonctionne

 

Si, comme moi, tu veux rajouter le roulis,tangage,lacet ou autre mouvement de corps c est dans position corps que ça se passe.

L amplitude des pas de chaque patte ou position "d'attitude" par exemple c'est dans position pattes  qu il faut le définir.

Le style de marche vecteur de marche et direction c est dans Mouvement

si tu rajoute des éléments a ton robot(un bras articulé par exemple) il faudra modifié la cinématique inversé pour qu elle le prenne en compte

 

et tu distribue tout ca avec une boucle

la définition est simple:

servo1 = GammaPatte(avant,droite)

servo2 = AlphaPatte(avant,droite)

servo3 = BetaPatte(avant,droite)

servo4 = GammaPatte(avant,gauche)

ect......

 

Un lien avec la cinématique fonctionnel, il te suffit de bouger les curseurs vert en haut de la page.Je n'ai pas résolu le déplacement visuel de la diagonal pour que tu vois bien que le pied n'est pas un point fixe sur la page mais que c est bien la cinématique qui gère les angles

 

https://www.geogebra.org/m/nU9jakAj

Voila

[Vaelan]

Petit precision parce que c est pas tres clair dans mes screen shoot :

DiffX et DiffY sont des valeurs absolu

[AlexBotMaker]

Effectivement, il me manquait beaucoup de chose ! Mdr

Bon je vais essayer de tout reprendre et essayer de comprendre se qui se passe avant de tout coder ! :-)

Mais en tout cas Merci d'avance pour ton aide ! C'est du beau taff !!

 

Dès que j'aurai programmé je posterai des vidéos du robot :-)

[Vaelan]

Si t'as besoin de précision sur des détails que tu comprends pas je suis dispo y'a qu'a demander

[AlexBotMaker]

Bon ben enfaite j'ai déjà tout codé lol

J'ai de nombreuse question ^^

#include <SoftwareSerial.h>
#include <Servo.h>
#include "Constante.h"

/*******************************************/
/*  ALEXANDRE - VAELAN   ROBOT-MAKER.COM   */
/*******************************************/  

/*Robot Quadrupède 3DOF:

-> Arduino UNO R3 
-> Mini Maestro 12
-> Batterie Accu 6V
-> Liaison série 
*/

          /*LB	  LF*/		
    	   /*	   */
    	    /******/
            /******///-----> FORWARD
    	    /******/
    	   /*      */
    	  /*RB    RF*/
    

//Avant droit Servos (1)
#define AVD0 0
#define AVD1 1
#define AVD2 2
//Arrière droit Servos (2)
#define ARD0 3
#define ARD1 4
#define ARD2 5
//Avant gauche Servos (3)
#define AVG0 6
#define AVG1 7
#define AVG2 8
//Arrière gauche Servos (4)
#define ARG0 9 
#define ARG1 10
#define ARG2 11

void setup()
{
  
  Serial.begin(9600);  //ecrire sur le port serie
  Maestro_Serial.begin(9600); //Liaisons série TX -> RX
  
  
}

void loop()
{
  
  lateral=0;
  longi=0;
  alti=5;
  
    //Test :
    for(int i =0; i<=1; i++)
    {
    //Avant droit Servos (1)
    Move_Servo(AVD0,Gamma_Patte);
    Move_Servo(AVD1,Alpha_Patte);
    Move_Servo(AVD2,Beta_Patte);
    //Arrière droit Servos (2)
    Move_Servo(ARD0,Gamma_Patte);
    Move_Servo(ARD1,Alpha_Patte);
    Move_Servo(ARD2,Beta_Patte);
    //Avant gauche Servos (3)
    Move_Servo(AVG0,Gamma_Patte);
    Move_Servo(AVG1,Alpha_Patte);
    Move_Servo(AVG2,Beta_Patte);
    //Arrière gauche Servos (4)
    Move_Servo(ARG0,Gamma_Patte);
    Move_Servo(ARG1,Alpha_Patte);
    Move_Servo(ARG2,Beta_Patte);
    }
}


/***********************CINEMATIQUE INVERSE SIMPLE (Longi,lateral, alti)*******************************/

 
int Vecteur(double x, double y, double z)    //Calcul de l'Hypothénuse (THEOREME DE PYTHAGORE)
 {
 Hypothenuse = sqrt(pow(pow(x,2)+pow(y,2)-COAX,2));
 L = sqrt(Hypothenuse + pow(z,2));
 Serial.print("Vecteur=");
 Serial.print(L,DEC);
 Serial.println();
 return L;
 
 }
 
int Calcul_Angle(double adj1,double adj2,double opp)   //Fonction pour calculer les angles (AL-KASHI)
 {
 Angle = acos((pow(adj1,2)+ pow(adj2,2)) - pow(opp,2)/ (2*adj1*adj2));
 return Angle;
 }
 
/***************************************************************/

int Angle_Alpha(double x,double y,double z)  //Calcul de l'angle ALPHA (ATTENTION la fonction Acos retourne des radians)Mouvement vertical
 {
 Alpha = acos(z/Vecteur(x,y,z))+Calcul_Angle(Vecteur(x,y,z),FEMUR,TIBIA);
 Alpha = Alpha*57,3;
 Serial.print("Alpha=");
 Serial.print(Alpha,DEC);
 Serial.println();
 return Alpha;
 }
 
int Angle_Beta(double x,double y,double z) //Calcul de l'angle BETA (ATTENTION la fonction Acos retourne des radians) Mouvement vertical
 {
 Beta = acos(Calcul_Angle(FEMUR,TIBIA,Vecteur(x,y,z)));
 Beta = Beta*57,3;
 Serial.print("Beta=");
 Serial.print(Beta,DEC);
 Serial.println();
 return Beta;
 }

int Angle_Gamma(double x, double y)  //Calcul de l'angle GAMMA (ATTENTION la fonction Acos retourne des radians) Mouvement horizontal
 {
  Gamma = atan(y/x)+ MontageServo;
  Gamma = Gamma*57,3;
  Serial.print("Gamma=");
  Serial.print(Gamma,DEC);
  Serial.println();
  return Gamma;
 }

/*************************POSITION CORPS ROBOT**************************/ 

int Calcul_OffsetX(double cote)
{
 OffsetX = (X_LONGEUR/2)*cote + longi;
 Serial.print("OffsetX=");
 Serial.print(OffsetX,DEC);
 Serial.println();
 return OffsetX;
}

int Calcul_OffsetY(double cote)
{
 OffsetY = (Y_LARGEUR/2)*cote + lateral;
 Serial.print("OffsetY=");
 Serial.print(OffsetY,DEC);
 Serial.println();
 return OffsetY;
}

int Calcul_OffsetZ()
{
 OffsetZ = alti;
 Serial.print("OffsetZ=");
 Serial.print(OffsetZ,DEC);
 Serial.println();
 return OffsetZ;
}

/******************************POSTION PATTES ROBOT******************************/

int Calcul_PiedX(double cote)
{
  PiedX = (Y_LARGEUR/2 + (COAX + EMPATTEMENT)*cos(90-MontageServo))*cote;
  Serial.print("PiedX=");
  Serial.print(PiedX,DEC);
  Serial.println();
  return PiedX;
}

int Calcul_PiedY(double sens)
{
  PiedY = (X_LONGEUR/2 + (COAX + EMPATTEMENT)*sin(90-MontageServo))*sens;
  Serial.print("PiedY=");
  Serial.print(PiedY,DEC);
  Serial.println();
  return PiedY;
}

int Calcul_PiedZ()
{
  PiedZ = HAUTEUR_CORPS/2;
  Serial.print("PiedZ=");
  Serial.print(PiedZ,DEC);
  Serial.println();
  return PiedZ;
}


/****************MOUVEMENT ROBOT***************************************************/

int Mouvement_DiffX(double cote)
{
  DiffX = abs(Calcul_PiedX(cote) - Calcul_OffsetX(cote));
  Serial.print("DiffX=");
  Serial.print(DiffX,DEC);
  Serial.println();
  return DiffX;
}

int Mouvement_DiffY(double sens)
{
  DiffY = abs(Calcul_PiedY(sens) - Calcul_OffsetY(sens));
  Serial.print("DiffY=");
  Serial.print(DiffY,DEC);
  Serial.println();
  return DiffY;
}

int Mouvement_DiffZ(double sens)
{
  DiffZ = abs(PiedZ + OffsetZ);
  Serial.print("DiffZ=");
  Serial.print(DiffZ,DEC);
  Serial.println();
  return DiffZ;
}

/****************************************************************/

int Mouvement_Alpha_Patte(double sens, double cote)
{
  Alpha_Patte = Angle_Alpha(Mouvement_DiffX(cote), Mouvement_DiffY(sens), Mouvement_DiffZ(sens));
  Serial.print("Alpha_Patte=");
  Serial.print(Alpha_Patte,DEC);
  Serial.println();
}

int Mouvement_Beta_Patte(double sens, double cote)
{
  Beta_Patte = Angle_Beta(Mouvement_DiffX(cote), Mouvement_DiffY(sens), Mouvement_DiffZ(sens));
  Serial.print("Beta_Patte=");
  Serial.print(Beta_Patte,DEC);
  Serial.println();
}

int Mouvement_Gamma_Patte(double sens, double cote)
{
  Gamma_Patte = Angle_Gamma(Mouvement_DiffX(cote), Mouvement_DiffY(sens))*cote*sens;
  Serial.print("Gamma_Patte=");
  Serial.print(Gamma_Patte,DEC);
  Serial.println();
}


/*****************PROTOCOLE*****************************/

void Move_Servo(unsigned char servo, unsigned int target){
	
	target = (map(target, 0, 180, 992, 2000) * 4); //Map the target angle to the corresponding PWM pulse.
	
	byte command[] = {
	PROTOCOL_BYTE, DEVICE_NUM, SET_TARGET_BYTE, servo, target & POLOLU_BIT_MASK, (target >> 7) & POLOLU_BIT_MASK};
	
	for(int i = 0; i < 6; i++)
	{
	Maestro_Serial.write(command[i]);
	}
}

void Set_Speed(unsigned char servo, unsigned int vitesse)
{
	byte command[] = {
	PROTOCOL_BYTE, DEVICE_NUM, SPEED_BYTE, servo, vitesse & POLOLU_BIT_MASK, (vitesse >> 7) & POLOLU_BIT_MASK};
	for(int i = 0; i < 6; i++)
	{
	Maestro_Serial.write(command[i]);
	}
}

#include <SoftwareSerial.h>
#include <Servo.h>

/*--------------------PROTOCOLE----------------------------*/
#define SET_TARGET_BYTE 0x04 // represents Pololu protocol set target command
#define DEVICE_NUM 0x0C // default device number for Pololu protocol
#define PROTOCOL_BYTE 0xAA // Pololu protocol identifier (0xAA)
#define POLOLU_BIT_MASK 0x7F
#define SPEED_BYTE 7
#define GETMOVING_STATE 0x13
/*--------------------Mini-Maestro 12----------------------*/
#define rxPin 3  // pin 3 connected to PololuSerial TX (not used in this example)
#define txPin 4  // pin 4 connected to PololuSerial RX
SoftwareSerial Maestro_Serial = SoftwareSerial(rxPin, txPin);
/*---------------XBEE S1-------------------------------*/
uint8_t pinRx = 12, pinTx = 11; //Xbee Tx/Rx
SoftwareSerial xbee(pinRx , pinTx);
/*------------------------------------------------*/

/***********CONSTANTE*************(cm)*/
const int COAX=3.6; 
const int FEMUR=5; 
const int TIBIA=10; 
const int Y_LARGEUR=8.6;
const int X_LONGEUR=11.1;     
const int HAUTEUR_CORPS=5.5;
const int DISTANCE_patte_G=6;
const int MontageServo=45;
/********VARIABLE**************/
double alti=0;
double lateral=0;
double longi=0;
double x=0;
double y=0;
double z=0;
double L=0;
double Hypothenuse=0;
double Angle=0;
double adj1=0;
double adj2=0;
double opp=0;
double Alpha=0;
double Beta=0;
double Gamma=0;
double cote=0;
double sens=0;
double OffsetX=0;
double OffsetY=0;
double OffsetZ=0;
double PiedX=0;
double PiedY=0;
double PiedZ=0;
double DiffX=0;
double DiffY=0;
double DiffZ=0;
double Alpha_Patte=0;
double Beta_Patte=0;
double Gamma_Patte=0;
/*********DEFINE**************/
#define HAUTEUR 9
#define EMPATTEMENT 7
#define Avant 1
#define Arriere 1
#define droite 1
#define gauche 1
#define bas 1
#define haut 1
/*********************/
#define NEUTRE 1500
#define FEMUR_HAUT 1900
#define FEMUR_BAS 1000
#define TIBIAS_HAUT 1300
#define TIBIAS_BAS 1550
#define COAX_AVDROIT 1900
#define COAX_ARRDROIT 1400
#define COAX_AVGAUCHE 1100
#define COAX_ARRGAUCHE 1600
#define TOURNER_DROITE 1100
#define TOURNER_GAUCHE 1900
/*******RAPIDE*******/
#define Temps 200
#define Temps2 220
#define Temps3 800
/*******LENT********
#define Temps 50
#define Temps2 60
#define Temps3 300
/****************/

Voila, il ya tout

1/ Pourquoi tu applique deux fois pythagore dans la fonction Vecteur ?? 

 

2/ Pourquoi tu rajoute z/Vecteur(x,y,z) a alpha ?

 

3/  Comment fonctionne sens et cote ?

 

4/ Les variable (x,y,z) ces moi qui doit les définir avec une valeur ???

 

5/ Ces quoi la différence entre lateral,longi,alti et (x,y,z) ??

[Vaelan]

 

1/ Pourquoi tu applique deux fois pythagore dans la fonction Vecteur ?? 

 

2/ Pourquoi tu rajoute z/Vecteur(x,y,z) a alpha ?

 

 

 

3/  Comment fonctionne sens et cote ?

 

4/ Les variable (x,y,z) ces moi qui doit les définir avec une valeur ???

 

5/ Ces quoi la différence entre lateral,longi,alti et (x,y,z) ??

sens,cote,x,y,z ne sont pas des variable mais des arguments de fonction  

 

si tu regardes bien, mon système mathématique n'est construit que de fonction qui s'appellent les unes les autres

et elles définissent seule comment appliquer la cinématique inverse a chaque angle selon la patte

 

Je l'ai écris pour l appeler le plus naturellement possible

tu veux l angle alpha de la patte avant droite = AlphaPatte(avant,droite)

tout le reste se fais tout seul :

AlphaPatte appelle la fonction AngleAlpha(x,y,z)

AngleAlpha prend 3 argument (qui sont 3 fonction)

   x =  la distance x entre l Offset et le pied qui n'est pas la même selon si elle est à droite ou a gauche (cote)

   y =  la distance y entre l Offset et le pied qui n'est pas la même selon si elle est à l'avant ou l'arrière  (sens)

   Z = pour toi pour l instant pas d argument mais ça viendra (le miens avec gyro dois savoir s il est sur le ventre ou sur le dos  il prend en argument haut et bas

ect ..... 

donc tu vois bien que quand tu appels la fonction AlphaPatte toutes les autres sont appelé automatiquement avec les bon arguments .

BetaPatte(avant,gauche) et BetaPatte(arriere,droite) ne renvoient pas le même résultat mais elles appellent les fonctions nécessaire avec les bon argument pour que ça corresponde bien a l angle beta de la patte avant gauche et arrière droite.

 

Latéral commande de déplacement latéral (droite gauche) du CORPS du robot

Longi   commande de déplacement longitudinale (avant, arrière) du CORPS du robot

Alti       commande de déplacement altitude(monter, descendre) du CORPS du robot

 

Alors je sais que quand tu vas comprendre parfaitement comment ça marche, tu vas voir une façon beaucoup plus simple de le faire marcher sans Offset et sans les différentiel, mais ton IK sera figé. Cette structure te laisse la porte ouverte pour faire ce que tu veux.

exemple, comme moi, module Position corps ,et juste pour la moitié des Offset de mon robot (je t'épargne les équations)

 

Et la tu devrais comprendre pourquoi je te dis de bien structurer ton programme parce que ça prend très vite beaucoup de place 

 

sur ce que tu a fais (je l ai juste survolé pas regardé en détail et je je suis pas bien placé pour t aider en codage je vais laisser ça à d autre) mais j ai quand même vu des erreur.

 

dans le loop (je pense que tu l auras compris ) toute ces fonctions non rien a faire la, elle sont appelé par:

AlphaPatte

BetaPatte

GammaPatte

et même ces fonctions la, devrait être appelé par une boucle sur l'asservissement des servo

 

les arguments de fonction (x,y,z,sens,cote) ne se déclare pas se sont des arguments

 

tu peux créer une fonction pour les "serial print"

 

y a des erreurs .

 

 

 

#define Avant 1
#define Arriere 1
#define droite 1
#define gauche 1
#define bas 1
#define haut 1

 

#define Avant 1
#define Arriere -1
#define droite 1
#define gauche -1
#define bas 1
#define haut -1

 

il faut bien comprendre qu'une valeur comme "largeur/2" vaut pour la droite et la gauche

avec la fonction : OffsetX(cote)=largeur/2*cote

quand tu appels :Valeur+OffsetX(droite)    c'est égale à Valeur+(largeur/2)

                            Valeur+OffsetX(gauche) c est égale à Valeur-(largeur/2)

 

je trouve ça trop "fouillis" (avis perso, peut être a tord)

 

int Mouvement_Alpha_Patte(double sens, double cote)
{
Alpha_Patte = Angle_Alpha(Mouvement_DiffX(cote), Mouvement_DiffY(sens), Mouvement_DiffZ(sens));
Serial.print("Alpha_Patte=");
Serial.print(Alpha_Patte,DEC);
Serial.println();
}

je ne sais pas coder mais ça devrais ressembler plutôt à ça:

A L'AIDE Mike 118

int Alpha_Patte(double sens, double cote)
    {
    Resultat = Angle_Alpha(DiffX(cote), DiffY(sens), DiffZ));
    return Resultat;
    }

Et ce pour toutes tes fonction

 

ça je comprends pas:

 

 

//Test :
for(int i =0; i<=1; i++)
{
//Avant droit Servos (1)
Move_Servo(AVD0,Gamma_Patte);
Move_Servo(AVD1,Alpha_Patte);
Move_Servo(AVD2,Beta_Patte);
//Arrière droit Servos (2)
Move_Servo(ARD0,Gamma_Patte);
Move_Servo(ARD1,Alpha_Patte);
Move_Servo(ARD2,Beta_Patte);
//Avant gauche Servos (3)
Move_Servo(AVG0,Gamma_Patte);
Move_Servo(AVG1,Alpha_Patte);
Move_Servo(AVG2,Beta_Patte);
//Arrière gauche Servos (4)
Move_Servo(ARG0,Gamma_Patte);
Move_Servo(ARG1,Alpha_Patte);
Move_Servo(ARG2,Beta_Patte);
}

 GammaPatte, AlphaPatte, BetaPatte, de quoi? y a pas d'argument pour l IK !

//indiquer à l'IK de quelle patte il doit calculer cette angle
Move_Servo(AVD0,Gamma_Patte(avant,droite));
Move_Servo(ARD0,Gamma_Patte(arriere,droite));
Move_Servo(AVG0,Gamma_Patte(avant,gauche));

Bien qu'en Python le code et plus court, je pense que même en C tu peux réduire énormément il y a beaucoup de chose inutile ou mal structuré tu devrais bien comprendre la base de cinématique inversé que je t ai faite avant d' essayer de la coder sinon tu arriveras pas à faire marcher ton robot.Comme je n'ai fais que survoler ton code y a peut être d'autre erreur mais je pense que tu vas le retravailler.

[AlexBotMaker]

Ok j'ai apporté certaine modification, en tout cas c'est ouf ton travaille ! chapeau bas !

Sinon j'ai encore un problème avec l'angle gamma je suis a 45deg partout au lieu de 90deg

Puis j'essaye de modifier les valeur alti et longi etc mais sa ne fait rien donc je pense que j'ai un souci quelque part au niveau du programme..

Je te tiens au jus si j'ai du nouveau !

[Mike118]

 

 

je trouve ça trop "fouillis" (avis perso, peut être a tord)

je ne sais pas coder mais ça devrais ressembler plutôt à ça:

A L'AIDE Mike 118

int Alpha_Patte(double sens, double cote)
    {
    Resultat = Angle_Alpha(DiffX(cote), DiffY(sens), DiffZ));
    return Resultat;
    }

Et ce pour toutes tes fonction

 

 

désolé du retard ... 
Je répond à l'appel que maintenant. 

Alors déjà je fais un point sur la situation histoire de bien vois si j'ai rien raté donc surtout corrigez moi si je me trompe . 

On parle d'un robot quadrupède, 
Les pattes sont définis par : avant, arrière, / droite , gauche    ( sens, coté ) 
exemple, patte, avant, droite.
 

chaque patte est composé de 3 servo, 
ces servo sont appelé alpha beta et gama. 

 

les trois morceaux de jambe formant une patte sont appelé coax fémure et tibia, 
Pour chaque patte
coax est  lié au corps via le servo gama , le fémure est lié à coax via beta et le tibia est lié au fémure via alpha

le but du jeu de la cinématique inverse fournie par vaelan : 

1) On fixe le bout des pieds de chacune des 4 patte du robot à une position donné 

Comment est elle fixée cette position ? je ne sais pas j'ai peut être raté ce point là expliqué à un moment donné dans la discussion.  Mais ok j'admet. c'est sans doute défini par l'empattement mais on ne va pas voir de ce côté pour le moment ça ne change pas la donne. 

à cette position le centre du robot est en   0, 0 , 0  en x y et z noté longi(tudinal/tude ?), latéral , alti(tude?) 

2) à partir de là, pour une position du centre du robot donné en  longi latéral et alti on est censé pouvoir faire une fonction qui va définir les angles des chacun des servo de chacune des pattes . 

Longi lateral et alti sont d'après la stratégie décrite des variables globales , position actuelle du robot,  qui sont appelé et mise à jour du coup dans les fonctions d'exécution je suppose.  // attention dans ce cas ne pas initialliser les variables dans le loop ! 

à partir de là on peut imaginer une fonction

alphaPatte( sens, cote ) qui retourne l'angle alpha, d'une patte donné (défini par  sens et côté qui peuvent valoir avant arrière pour sens, gauche droite pour côté ) 

idem pour betaPatte  gamaPatte . 

3) Du coup on fait la cinématique inverse  pour faire ces fonctions là, etvaelan a fait tous les calculs, vraisemblable correct vu que le simulateur sur géogébra semble fonctionner à merveille ( encore une fois, beau boulot ) 

Du coup, on regarde bien comment vaelan a fait les formule, et il faut maintenant les appliquer correctement en fonction des pattes auquel cela s'applique. 

Le premier point à bien voir est le choix de symétrie fait concernant les angle du robot fait que l'on peut faire le calcul pour une patte avec des variables intermédiares DiffX et DiffY ( qui se calcul de la position longi et lateral  et en fonction de la patte où on est. ) , on peut faire le cas pour x et y positif ( patte avant droite ) et par symétrie faire le cas ou x et ou y sont négatif, cas des autres pattes, et qui se calcule à partir de la même formule pour pour la patte avant gauche mais avec des diffX et diffY différent. 

Du coup il est important de bien comprendre que sont DiffX et DiffY ( voir le schémas de valean ) 



du coup on se rend compte que pour la patte avant droite on a : 

DiffX ( patte avant droite ) = DiffX0 - lateral
DiffY ( patte avant droite)  = DiffY0 - longi  

alors que pour la patte arrière  droite à la même position lateral et longi on aura 
DiffX ( patte arrière droite ) = DiffX0 -lateral 
DiffY ( patte arriere droite ) = DiffY0+ longi 

Et on complète encore : 

DiffX ( patte avant gauche) = DiffX0 +lateral 
DiffY ( patte avant gauche) = DiffY0- longi 

 

DiffX ( patte arrière droite ) = DiffX0 +lateral 
DiffY ( patte arriere droite ) = DiffY0+ longi 

 

et dans les formules de vaelan, ce qui est noté x et y en fait c'est bien ces diffX et diffY qu'il faut mettre pour chaque patte et qui donne ensuite l'angle alpha beta et gama ... 

4) On va faire une jolie formule pour le calul de ces DiffX et DiffY. On voit en fonction des pattes que DiffX est uniquement fonction de si tu es droite ou gauche et diffY est uniquement fonction de si tu est avant arrière, 

Du coup on peut écrire DiffY(sens) = DiffY0 + longi si sens == arriere, DiffY0-longi si avant, 
et DiffX(cote) = DiffX0 + lateral si cote ==gauche , DiffX0-lateral si droite 

Après pour écrire cela on peut donc le faire de plusieurs façon différentes, un if else, vu que je l'ai mis sous cette forme au dessus, ou on faire un calcul se basant sur la valeur du " sens " et du "côté", et pour cela il faut choisir jusdicieusement les valeurs de sens et de côté pour que cela facilite les calculs. 

Si on fait 

#define ARRIERE 1

#define AVANT -1
#define GAUCHE 1 

#define DROITE -1 

on obtient bien DiffY(sens) = DiffY0 + sens*longi ;                             sens pouvant valoir ARRIERE ou AVANT
                          DiffX(cote) = DiffX0 + cote*lateral  ;                             cote pouvant valoir DROITE ou GAUCHE

Il n'y a pas forcément une meilleur façon de calculer la chose, l'une peut être plus pratique l'autre être moins source d'erreur .. 

à faire en fonction de la préférence, et en plus ce ne sont pas les seules façon de faire ce calcul ... 

Moi d'ailleurs je suis passé par un DiffY0 et un DiffX0 , les valeurs du DiffX et DiffY pour longi et lateral à 0 ... mais ça peut se calculer en fonction de la taille du robot et de l'empatement choisit qui peut enfait être une variable ... 

 

5) après il faut faire la fonction qui pour chaque patte met bien à jour les valeurs des servo des pattes avec les bons angles calculés

un truc du grenre : 

 

updatePatte(i ) {

Setservodeg( 3*i , AlphaPatte ( sens(i) , cote(i) );

Setservodeg( 3*i+1 , BetaPatte ( sens(i) , cote(i) );

Setservodeg( 3*i+2 , BetaPatte ( sens(i) , cote(i) );

}

 

et Setservodeg( num,angle){

setServo(num , map( angle, Min(num], Max(num), mindeg(num), maxdeg(num) ); 

 

}

6) Une fois ces fonctions faites tu peux imaginer de faire ton code comme suit : 

 

 

struct POSITION {

 

 float longi;

 float lateral;

 float alti;

 

}

 

POSITION  tableDePosition[10]

 

setup{

 

// initialisation des positions 

tableDePosition[0].longi=0;

tableDePosition[0].lateral=0;

tableDePosition[0].alti=0;

 

 

// continuer avec les 9 autre position

}

loop() 

{

   for i =0; i<10; i++ 

   updateServo( tableDePosition[i]);

 

}
 

et après tu rentre au fur et à mesure dans les fonctions plus bas niveau... 

updateServo()

{

    for i  0->3 updatePatte(i ) ; 

}

 

 

 

Bon je m'excuse, j'ai un peu bâclé la fin mais vu l'heure ( 5H du mat après cet edit ) et le fait qu'en arrivant à ce point je sois en train de me demander : hum ... c'était quoi la question ?  je me dis qu'il vaut mieux que je m'arrête là ^^ 

Du coup vous me direz si j'ai répondu à la question, si j'ai éclairé ou assombris le problème, et n'hésité pas au passage à répondre aux quelques questions qui se sont glissé dans ce long message ( à repérer grâce au point d'exclamation ) et à me corriger si j'ai dit une boulette quelque part ! 

[Vaelan]

moi j ai fais un petit truc pour une base de travail (désolé pour mon niveau de programmation les critiques son les bienvenu)

 

Pour mon exemple j ai utilisé les onglet ("fichier") disponible avec l IDE arduino ,le trie se fait par ordre alphabétique donc j ai placé une lettre avant le nom voulu pour les mettre dans le bonne ordre

 

Ouvres le moniteur serie pour tester le programme 

 

 

Ouvres le moniteur serie pour tester le programme

Voici les différent onglet ("fichier") que j ai créé:

 

A_declaration

/***********CONSTANTE**************/
const float coax = 3.6;
const int femur = 5;
const int tibia = 10;
const float largeur = 8.6;
const float longueur = 11.1;
const float hauteur_corps = 5.5;
const float montageServo = radians(45);

/********VARIABLE**************/
int aff = 0;
float alti = 0;
float lateral = 0;
float longi = 0;

/*********DEFINE**************/
#define empattement 7
#define avant 1
#define arriere -1
#define droite 1
#define gauche -1

 

 B_Cinematique_inverse:

/*Calcul d'angle de cinématique inversé pour patte à 3 dgrés de liberté*/
/*---------------------------------------------------------------------*/
//Ici on gère les servo 
// exemple : robot type fourmis on rajoute un servo entre le thorax et la queue
// je te le donne en etat de marche mais pas cles en main pour que tu comprenne son fonctionnement
//pour pouvoir faire tout ce que tu veux avec.
// Un indice sur ton premier probleme ton vecteur ne peut pas etre plus long que ta patte tendu! ;) 


//Calcul de la longueur du vecteur Alpha => Pied
//Base calcul = THEOREME DE PYTHAGORE
float Vecteur(float x, float y, float z) {
  float Resultat = sqrt(pow((sqrt(pow(x, 2) + pow(y, 2)) - coax), 2) + pow(z, 2));
  return Resultat;
}

//Calcul d'angles avec AL-KASHI(théorème de Pythagore généralisé)
float Angle(float adj1, float adj2, float opp) {
  float Resultat = acos((pow(adj1, 2) + pow(adj2, 2) - pow(opp, 2)) / (2 * adj1 * adj2));
  return Resultat;
}

//Calcul de l'angle ALPHA
//Rotation sur le plan (X,Z)
float AngleAlpha(float x, float y, float z) {
  float Alpha = (acos(z / Vecteur(x, y, z)) + Angle(Vecteur(x, y, z), femur, tibia));
  return Alpha;
}

//Calcul de l'angle BETA
//Rotation sur le plan (X,Z)
float AngleBeta(double x, double y, double z) {
  float Beta = Angle(femur, tibia, Vecteur(x, y, z));
  return Beta;
}

//Calcul de l'angle GAMMA
//Rotation sur le plan (X,Y)
float AngleGamma(float x, float y) {
  float Gamma = atan(y / x) + montageServo;
  return Gamma;
}

C_Position_Corps:

/*************************POSITION CORPS ROBOT**************************/
/*Référentiel du chassis*/
//Ici (on oublie qu il a des pattes ;))et on definie la position du thorax
//exemple : on rajoute un gyroscope avec roulis et tangage on pourra lui demander
//          de garder son plateau toujours de niveau que le sol soit en pente ou pas   

float OffsetX(int cote) {
  float Resultat = (largeur / 2) * cote + lateral;
  return Resultat;
}

float OffsetY(int cote) {
  float Resultat = (longueur / 2) * cote + longi;
  return Resultat;
}

float OffsetZ() {
  float Resultat = alti;
  return Resultat;
}

D_Position_Patte:

/******************************POSTION PATTES ROBOT******************************/
/*Référentiel des pieds*/
//ici on definie ou doivent etre les pied (fixe dans mon exemple)
//exemple :longeur et hauteur des pas pour la marche

float PiedX(int cote) {
  float Resultat = (largeur / 2 + ((coax + empattement) * cos(radians(90) + montageServo)))* cote ;
  return Resultat;
}

float PiedY(int sens) {
  float Resultat = (longueur / 2 + ((coax + empattement) * sin(radians(90) + montageServo))) * sens;
  return Resultat;
}

float PiedZ() {
  float Resultat = hauteur_corps / 2;
  return Resultat;
}

E_Mouvement:

/****************MOUVEMENT ROBOT***************************************************/
/*Différentiel pour la cinématique inversé*/
//ici il s'agit du différenciel entre le thorax et le pied et donc la patte en elle meme
//exemple : tu rajoute un bras articulé c est ici que tu géres a quelle distance ce trouve
//          la main de ton bras du thorax

float DiffX(int cote) {
  float Resultat = abs(PiedX(cote) - OffsetX(cote));
  return Resultat;
}

float DiffY(int sens) {
  float Resultat = abs(PiedY(sens) - OffsetY(sens));
  return Resultat;
}

float DiffZ() {
  float Resultat = abs(PiedZ() + OffsetZ());
  return Resultat;
}

float Alpha_Patte(int sens, int cote) {
  float Resultat = degrees(AngleAlpha(DiffX(cote), DiffY(sens), DiffZ()));
  return Resultat;
}

float Beta_Patte(int sens, int cote) {
  float Resultat = degrees(AngleBeta(DiffX(cote), DiffY(sens), DiffZ()));
  return Resultat;
}

float Gamma_Patte(int sens, int cote) {
  float Resultat = degrees(AngleGamma(DiffX(cote), DiffY(sens)) * cote * sens);
  return Resultat;
}

F_TEST

//ici je me suis pas pris la tete ;))

void instruction() {
  Serial.println("Instruction de commande disponible :");
  Serial.println("commande Alti +/- => z/a");
  Serial.println("commande Lateral +/- => s/q");
  Serial.println("commande Longi +/- => x/w");
}

void reception() {
  char c = Serial.read();
  if (c != -1) {
    switch (c) {
      case 'z':
        alti = alti + 1; break;
      case 'a':
        alti = alti - 1; break;
      case 's':
        lateral = lateral + 1; break;
      case 'q':
        lateral = lateral - 1; break;
      case 'x':
        longi = longi + 1; break;
      case 'w':
        longi = longi - 1; break;
      default:
        instruction();
    }
    aff = 1;
  }
}

void Patte(int sens, int cote) {
  Serial.println(Alpha_Patte(sens, cote));
  Serial.println(Beta_Patte(sens, cote));
  Serial.println(Gamma_Patte(sens, cote));
}

void envoie() {
  if (aff == 1) {
    aff = 0;
    Serial.println("Patte avant droite");
    Patte(avant, droite);
    Serial.println("Patte avant gauche");
    Patte(avant, gauche);
    Serial.println("Patte arriere droite");
    Patte(arriere, droite);
    Serial.println("Patte arriere gauche");
    Patte(arriere, gauche);

  }
}

G_Process

//sans commentaire ;)

void setup() {
  Serial.begin(9600);
  instruction();
}

void loop() {
  reception();
  envoie();
}

Voila j'espère que ça va t'aider

 

 

 

 

 

 

[AlexBotMaker]

Nickel chrome ton programme

juste "aff" c'est quoi ? sa n'apparaissait pas avant

Puis y'a effectivement le faite que je mettais pas en radians comme ta fait radian(90) ! Donc sa pourrait expliquer des valeurs incorrect. Pour les différent onglet j'ai pas fait sa parce que a un moement donnée j'avais l'impression que les informations ne passait pas pck les angle restait bloquer a 90deg meme si je changeais alti,longi,lateral donc sa me rendait dingue lol je comprenais pas donc j'ai regarder que mon programme passait bien dans toute les méthode puis voila j'en suis la pour l'instant.

 

Merci mike118 pour ton avis sur le programme ! Je vais tenir compte des remarques, je me rapproche du but.

Par contre concernant le mappage des servos c'est dans ma partie protocole pour mon contrôleur de servomoteur que c'est directement définie. 

[Vaelan]

"aff" c'est juste une variable qui sert d interrupteur pour afficher les données une seul fois (vu que la fonction est appelé dans le "loop")quand on envoie une commande.

 

je préfère la "Programmation Orienté Objet" c'est pour ça que j'ai utilisé cette structure, tu peux décider de modifier le comportement du thorax dans "Position_corps" sans te préoccuper du fonctionnement de l'IK ou des pieds du robot idem pour les autres modules ,quelque soit le comportement du thorax ou des pieds l'IK gère ce qu il se passe entre les deux.

 

je pense que ça peu être la racine du programme de ton robot mais tu dois la comprendre et la maîtriser sur le bout des doigt et tu verras que tu peux tout faire avec . Un robot sur roue avec des capteur, le faire tourner selon les obstacle c'est "facile", avec nos robot à pattes ça devient tout aussi "facile" avec la cinématique inversé.

 

La partie "TEST" n'est juste la que pour te montrer que ça marche avant de le mettre sur ton robot et j ai calibré mes commande sur 1 unité(cm)pas asses fin pour des beau mouvement et  c'est a remplacer par ton "protocol" maintenant que tu as les angles en degrés un simple "MAP" pour envoyer les données a tes servo .

 

ATTENTION c'est pas complet je ne t es pas mis les correction IK , limitation physique,Tempo, les genres de pas et les styles de marche  de ton robot pour deux raison :

 

1 ) je les connais pas toutes et c est a toi de décider ce que dois faire ton robot.

 

2 ) j' ai longtemps bloqué sur des détails ou une mauvaise approche parce que je travaillé seul dessus, ta vision n'est pas la mienne donc je compte sur toi pour m'en m'apporter de nouvelle ou une nouvelles approche (D’ailleurs merci Mike 118 niveau prog. je pense que ta solution va m apporter beaucoup).

 

 

 

 

[Vaelan]

 Je suis très intéressée par la méthode de Mike118 y a juste une chose très importante! une commande ne se soustrait JAMAIS.

 

DiffX ( patte avant droite ) = DiffX0 - lateral

 DiffY(sens) = DiffY0 + sens*longi ;

 

Quand on fais une commande +1 tout les offset se déplace dans la même direction donc c est +1 pour tous les offset.C est l Offset d'origine qui doit être positif ou négatif selon le cote ou le sens.

 

 

// initialisation des positions 

tableDePosition[0].longi=0;

tableDePosition[0].lateral=0;

tableDePosition[0].alti=0;

Ça je comprend pas .... Si c'est pas une erreur je suis très intéressé (ça ressemble beaucoup a un objet)

mais l approche des tableau peu être très  intéressante.

 

 

 

 ce qui est noté x et y en fait c'est bien ces diffX et diffY

Ca c'est OK. J'ai fais la différence parce que la partie "Cinematique_inverse" peut servir pour d'autre chose en créant un nouveau module utilisant la "Cinématique_inverse" je voulais que ça reste clair au niveau de "x,y,z" et ou les "Diff" n'auraient pas de sens.

[Mike118]

 y a juste une chose très importante! une commande ne se soustrait JAMAIS.

 

Pourquoi donc ? 

Règle vu en cours ? 

Raison d'expérience personnelle ? 

Personnellement je ne suis pas d'accord ^^ Donc soit j'ai mal compris la remarque soit je veux bien plus d'explications ^^ 

Et j'ai pas compris ta remarque  : 

 

 

 

Quand on fais une commande +1 tout les offset se déplace dans la même direction donc c est +1 pour tous les offset.C est l Offset d'origine qui doit être positif ou négatif selon le cote ou le sens.

 

 

 

 

Ensuite, concernant : 

 

 

 

Ça je comprend pas .... Si c'est pas une erreur je suis très intéressé (ça ressemble beaucoup a un objet)

mais l approche des tableau peu être très  intéressante.

 

 

 

non ce n'est pas une erreur, il s'agit de la mise en place d'une structure, donc oui ça permet d'avoir en quelque sorte "de l'orienté objet" qui peut être représenté comme étant la définissant une " classe " qui aurait,  "que des attributs" ... 

Voir mon petit tutoriel ici  Si tu as des questions hésites pas !

[Vaelan]

 

 

Voir mon petit tutoriel ici  Si tu as des questions hésites pas !

Super le tuto je connaissais pas . Tu viens de me réconcilier avec arduino et du coup on peux optimiser le code que j ai fournis plus haut

 

 

Concernant la "règle" qui dis qu'on ne soustrait jamais une commande, c'est juste le fameux théorème de vaelan qui s applique à sa cinématique inversé qui dis : Si -(commande) alors démonte ton robot et remonte le comme il faut ) Blague a part , les commandes définissent un vecteur d'origine 0,0,0 (le centre du robot), sur (x,y ou z) , de direction - ou + et de longueur la valeur de la commande.

Donc si tu as un latéral de -1  c est un vecteur sur x vers la gauche de longueur 1 maintenant si tu fais -vecteur tu inverse le vecteur du coup si tu dis a un offset de droite +vecteur et a un offset de gauche -vecteur tu vas casser quelque chose ) J ai voulu faire une explication simple mais je me rends compte qu avec un langage plus matheux ça aurais était peut-être un peu plus clair, désole.

 

alors t en ai ou AlexBot? ca avance? 

[Mike118]

hum, je crois que j'ai pas bien compris ton explication... 

Du coup je vais essayer de donner  un exemple avec des chiffres ... 

Supposons qu'avec un empattement défini je me met à Z =0  X=0 Y=0 . 
 

Dans cette position on a par exemple DiffX = DiffX0= 10 par exemple.

Si je veux bouger le centre du robot dans de DX=1  suivant x je vais bien avoir DiffX patte droite (avant ou arrière ) = 9 = DiffX0 -DX 
Et pour les pattes gauche  DiffX=11 = DiffX0 + DX ... 

 

Du coup faut bien que je soustraie la consigne à un moment donné =)

 

[Vaelan]

En faite tu confonds les référentiel et les différentiel les commandes n'agissent pas directement sur DiffX mais plutot sur OffsetX (pas encore de commande sur les pieds mais Alex va les rajouter j en suis sur )  . la j ai pas le temp , je te fais un schéma ce soir

[Vaelan]

Désole un peu en retard pour l explication

 

 

 

Si je veux bouger le centre du robot dans de DX=1  suivant x je vais bien avoir DiffX patte droite (avant ou arrière ) = 9 = DiffX0 -DX 
Et pour les pattes gauche  DiffX=11 = DiffX0 + DX ... 

 

En faite ma cinématique ne marche pas comme ça . Ce qui définît l'empattement c'est les DiffX,Y ou Z (Différentiel) a partir des Offset et position des pieds (Référentiel) .

 

Alors oui y a bien un empattement définit par défaut , mais il faut bien une position de départ

 

Le but étant de ne contrôler que le centre du robot, chaque commande peut être considéré un vecteur partant de Z=0  X=0 Y=0

un latéral de +1 c est un vecteur d'origine  (Z=0  X=0 Y=0) à ( Z=0  X=1 Y=0)

un latéral de -1 c est un vecteur d'origine  (Z=0  X=0 Y=0) à  (Z=0  X=-1 Y=0)

donc on aura toujours "Offset + commande"

exemple pour un latéral de -1:

Offset translaté avant droite = Offset physique + vecteur (( 0 , 0 , 0) => ( 0 , -1 , 0))

Offset translaté avant gauche = -Offset + vecteur (( 0 , 0 , 0) => ( 0 , -1 , 0))

On sait que le vecteur est définit par la droite de l axe x du plan (x,y) passant par le centre du robot on peut donc écrire:

Offset translaté arrière gauche = -Offset + (-1) 

Offset translaté arrière droite = Offset +(-1)

 

Je vais pas dire "on fait la même chose pour la marche" parce qu il faut quelque chose d'un peu plus complet mais on garde le même principe, et  tout nos référentiel (Offset et Pied) sont calculé.

 

Le différentiel entre A et B (nos référentiel) permet de calculer l'empattement dynamiquement 

 

Et enfin  les trois différentiel permettent de calculé tout les angles

 

 

Avec ce début d IK j'ai pas d'exemple qui me viens a l esprit mais sur mon robot j'ai prévu en plus le roulis, tangage, et lacet (avec des méthode de marche bien entendu et d'autre chose .... ) je compte lui monter, pour ceux qui connaissent, un "Flymentor" c' est un gyro 3 axes avec accéléromètre et capteur de mouvement au sol (recupéré sur un de mes hélico) il va gérer le lacet et le tangage pour un mise a niveau du thorax.Grace à se système de différentiel quelque soit la correction d assiette du gyro qui sera appliqué, je pourrais toujours utiliser mon latéral ou le faire marcher sans que les différents mouvement soit en conflit les uns avec les autres (dans les limite physique du robot). 

[Path]

J'aime bien les choses simples. Ça me plait cette simplification mais je comprends pas l'histoire de la soustraction. 

C'est moi où bien.

[Vaelan]

 

Offset translaté arrière gauche = -Offset + (-1) 

Offset translaté arrière droite = Offset +(-1)

Je suppose que c est ça que tu comprends pas?

 

en faite ca correspond a :

Offset translaté arrière gauche = -Offset + (lateral) 

Offset translaté arrière droite = Offset +(lateral)

 

Au final la radio (ou autre moyen de controle) ne dois controler que le centre du robot. 

sur une radio les manches symbolise le centre du robot.

continuons avec l'exemple de commande latéral:

coordonnée 0,0,0

coordonnée 0,5,0

coordonnée 0,-5,0 

[Vaelan]

On va continuer et développer ma logique d'IK plutot sur mon post parce que ca devient un peu trop envahissant pour AlexBotMaker et je développerais un peu plus sur la marche et un point important , dont j ai fais allusion au début de mon explication, à savoir son centre de gravité.

 

vieartificielle

 

 

Ce robot restera en équilibre à la condition que la projection de son centre de gravité soit en permanence dans le polygone de sustentation. Ce polygone est délimité par tous les points de contacts au sol.

 

Je reste  dispo si tu a des problème avec ton robot Alex