Go Down

Topic: Delta Robot (Read 1 time) previous topic - next topic

jonjon

Bonjour débutant j'ai réalisé un petit delta robot qui fonctionne dans un plan triangulaire avec des câbles (et non des bras classiques).
J'ai trois servos qui font bouger un point (pointeur) sur un axe xyz. Je contrôle la vitesse (qui doit être lente pour mon projet) et la hauteur via des variables et boutons.

Cela fonctionne bien mais les servos se positionnent un après l'autre, ce qui fausse la hauteur z du pointeur, je souhaiterai donc qu'ils se positionnent en même temps et dans des proportions égales. J'ai essayé de passé par un tableau mais cela ne fonctionne pas.
Si quelqu'un a une idée je suis preneur.

Code: [Select]
/*
untités :
- cm
- degres
*/

#include "math.h"               // importer la blibliothèque Math
#include <Servo.h>

Servo myservo1;  // create servo object to control a servo 
Servo myservo2;  // create servo object to control a servo
Servo myservo3;  // create servo object to control a servo

int xp= 10;
int yp= 10;
long randNumber_x;
long randNumber_y;

int timer = 10;
int servoArray[] = {1, 2, 3};
int count = 0;

float hauteur = 190;
float x1 = 0;
float y1 = 70;
float x2 = 64.5;
float y2 = -35;
float x3 = -64.5;
float y3 = -35;
float  h1;
float  h2;
float  h3;
float  hf1;
float  hf2;
float  hf3;
int v1;
int v2;
int v3;

int pos1 = 1;    // variable to store the servo position
int pos1final;
int pos2 = 1;    // variable to store the servo position
int pos2final;
int pos3 = 1;    // variable to store the servo position
int pos3final;

int boutonPoussoir_start = 8;   // … et du bouton poussoir
int boutonPoussoir_plus = 10;   // … et du bouton poussoir
int boutonPoussoir_moins = 9;   // … et du bouton poussoir

void setup()                    // exécuté une fois, au démarrage du programme
{
  Serial.begin(9600);           // initialise la blibliothèque Serial à 9600 bps

  pinMode(8, INPUT); // met la broche 2 en sortie
  pinMode(9, INPUT); // met la broche 2 en sortie
  pinMode(10, INPUT); // met la broche 2 en sortie
  pinMode(11, INPUT); // met la broche 2 en sortie
 
  myservo1.attach(2);  // attaches the servo on pin 9 to the servo object
  myservo2.attach(3);  // attaches the servo on pin 9 to the servo object
  myservo3.attach(4);  // attaches the servo on pin 9 to the servo object
  myservo1.write(pos1);
  myservo2.write(pos2);
  myservo3.write(pos3);

  delay(5000);                       // waits
}

void loop()                // nécessaire, même vide
{
  /*if (digitalRead (boutonPoussoir_plus) == HIGH)   // si le bouton + est appuyé 
  {
  }
  else
  {
    hauteur = (hauteur + 1);
    Serial.print("hauteur=");
    Serial.println(hauteur);
    delay(1000); // pause de 1s
  }
    if (digitalRead (boutonPoussoir_moins) == HIGH)   // si le bouton - est appuyé 
  {
  }
  else
  {
    hauteur = (hauteur - 1);
    Serial.print("hauteur=");
    Serial.println(hauteur);
    delay(1000); // pause de 1s
  }*/
 
 
 
  Serial.println("Calculons une hypothenuse");
  randNumber_x = random(-60, 60);
  randNumber_y = random(-60, 60);
  Serial.println(randNumber_x);
  Serial.println(randNumber_y);

  int xp = randNumber_x;
  int yp = randNumber_y;


// calcule la 1ere hypotenuse (possition du pointeur)
//h1 = distance entre le pointeur et la base du pied 1
  h1 = sqrt(sq(xp-x1)+sq(yp-y1));
  h2 = sqrt(sq(xp-x2)+sq(yp-y2));
  h3 = sqrt(sq(xp-x3)+sq(yp-y3));
  //h1 = xp-x1;
  Serial.print("h1 = ");
  Serial.println(h1);
  Serial.print("h2 = ");
  Serial.println(h2);
  Serial.print("h3 = ");
  Serial.println(h3);

// calcule de la longeur de fils 
  hf1 = (sqrt(sq(h1)+sq(hauteur))-hauteur);
  hf2 = (sqrt(sq(h2)+sq(hauteur))-hauteur);
  hf3 = (sqrt(sq(h3)+sq(hauteur))-hauteur);
  Serial.print("hf1 = ");
  Serial.println(hf1);
  Serial.print("hf2 = ");
  Serial.println(hf2);
  Serial.print("hf3 = ");
  Serial.println(hf3);

//calcule de l'angle pour le cercle de
  pos1final = (180*hf1)/56.5;
  pos2final = (180*hf2)/56.5;
  pos3final = (180*hf3)/56.5;
  Serial.print("pos1final deg= ");
  Serial.println(pos1final);
  Serial.print("pos1 deg= ");
  Serial.println(pos1);
  Serial.print("pos2final °= ");
  Serial.println(pos2final);
  Serial.print("pos3final °= ");
  Serial.println(pos3final);

  // ------------- servo 1
 
 
  for (count=0;count<3;count++) {
      Serial.print("---servoArray[count] °= ");
      Serial.println(servoArray[count]);
      for(pos(servoArray[count]) = pos(servoArray[count]); pos1 <= pos1final; pos1 += 1)     // goes from 180 degrees to 0 degrees
      {       // in steps of 1 degree
     
      myservo1.write(pos1);              // tell servo to go to position in variable 'pos'
      Serial.print("pos1 a = ");
      Serial.println(pos1);
      delay(timer);                       // waits
      }
     
     
  }
 
  if (pos1 < pos1final)
  {
    for(pos1 = pos1; pos1 <= pos1final; pos1 += 1)     // goes from 180 degrees to 0 degrees
    {       // in steps of 1 degree

      myservo1.write(pos1);              // tell servo to go to position in variable 'pos'
      Serial.print("pos1 a = ");
      Serial.println(pos1);
      delay(timer);                       // waits
    }
  }
  else
  {
    for(pos1 = pos1; pos1 >= pos1final; pos1 -= 1)     // goes from 180 degrees to 0 degrees
    {                                  // in steps of 1 degree
      myservo1.write(pos1);              // tell servo to go to position in variable 'pos'
      Serial.print("pos1 b = ");
      Serial.println(pos1);
      delay(timer);                       // waits
    }
  }

  // ------------- servo 2
  if (pos2 < pos2final)
  {
    for(pos2 = pos2; pos2 <= pos2final; pos2 += 2)     // goes from 180 degrees to 0 degrees
    {       // in steps of 1 degree

      myservo2.write(pos2);              // tell servo to go to position in variable 'pos'
      Serial.print("pos2 a = ");
      Serial.println(pos2);
      delay(timer);                       // waits
    }
  }
  else
  {
    for(pos2 = pos2; pos2 >= pos2final; pos2 -= 2)     // goes from 180 degrees to 0 degrees
    {                                  // in steps of 1 degree
      myservo2.write(pos2);              // tell servo to go to position in variable 'pos'
      Serial.print("pos2 b = ");
      Serial.println(pos2);
      delay(timer);                       // waits
    }
  }
  // ------------- servo 3
  if (pos3 < pos3final)
  {
    for(pos3 = pos3; pos3 <= pos3final; pos3 += 1)     // goes from 180 degrees to 0 degrees
    {       // in steps of 1 degree

      myservo3.write(pos3);              // tell servo to go to position in variable 'pos'
      Serial.print("pos3 a = ");
      Serial.println(pos3);
      delay(timer);                       // waits
    }
  }
  else
  {
    for(pos3 = pos3; pos3 >= pos3final; pos3 -= 1)     // goes from 180 degrees to 0 degrees
    {                                  // in steps of 1 degree
      myservo3.write(pos3);              // tell servo to go to position in variable 'pos'
      Serial.print("pos3 b = ");
      Serial.println(pos3);
      delay(timer);                       // waits
    }
  }

  Serial.println("-----------------------");
  delay(50000); // pause de 5s
}

skizoh

Salut, ton code est pas si mal indenté et pourtant c'est un enfer à lire je trouve ^^'  j'ai pas bien compris ou tu voulais en venir avec tes tableau sachant qu'il n'y en a pas un seul dans ton prog, bref mais pour avoir un mouvement simultané il faut que tu face des position intermédiaire et que tu les bouge un par un petit a petit non?

Skizo !
Un tien vaux mieux que deux tu l'auras !

vince3838

salut !
pour avoir un mouvement simultané, je pense qu'il faudrait piloter tes servos dans une seule boucle..
sinon, pour ce qui est du fonctionnement du delta, je pourrai plus t'aider en te disant de jeter un coup d'œil du coté de l'algorithme de bresenham.
car en plus de faire marcher tes servos en même temps, il faudra ajuster leurs vitesses en fonction de la course a parcourir pour que tes mouvements se terminent en même temps.
je suis en train de finaliser mon robot delta aussi, mais avec des steppers, et le simultané ne pose aucun problème.. donc ça devrait marcher avec des servos..
je suis au taf la mais ce soir ou demain je posterai un exemple de mon code.

si tu veux des infos n'hésite pas.

jonjon

Salut skizoh,
Merci, oui comme je l'ai dit, je début et en plus je suis de formation webdesigner, donc pas trop pur codeur ;) je découvre un peu et c'est pour cela que c'est très basic.
Je suis d'accord  avec toi sur le fait que je dois créer des étapes intermédiaires, mais cela va encore alourdir mon code.
Pour l'histoire des tableaux, j'imaginais que de mettre mes variables dans un tableau ou de faire des boucles allégerai mon code, mais je n'ai pas réussi à maitriser encore ces éléments donc je ne les ai pas mis dans le code posté.


Salut Vince
Merci pour ta proposition, j'ai déjà regarder à du code d'autre delta robot, mais pour dire vrai je n'ai rien compris, il était un peu trop avancé pour moi alors j'ai développé le mien.

vince3838

salut,

Quote
j'ai déjà regarder à du code d'autre delta robot, mais pour dire vrai je n'ai rien compris, il était un peu trop avancé pour moi alors j'ai développé le mien.


En fait je ne te parlais pas du calcul de la distance à parcourir, j'ai vu que tu avais tes propres calculs, je te parlais uniquement des mouvements simultanés une fois que tu a le résultat de tes 3 distances a parcourir.

skizoh

salut, alors pour cette histoire a monj avis le plus simple c'est de décomposé en tableau comme tu a dit donc faire quelque chose du genre:

Code: [Select]

int pos_final=...;
int val1[pos_final],val2[pos_final],val3[pos_final];  //3 tableau de pose_final x position si ton pas est de 1
int i=0;

while (i<pos_final)      // calcule de tes tableau de valeurs intermédiaire
{
i++;
val1[i]=i;
val2[i]=i;  // ta variable qui augmente donc tes position intermédière, 1..2..3....pos_final..
val3[i]=i;
}
i=0;
//mouvement

while (i<15)
{
myservo1.write(val1[i]);
myservo2.write(val2[i]);   // mouvement de chaque moteur avec les valeur intermédiaire du tableau
myservo3.write(val3[i]);
}




un truc dans ce gout la, je sais pas si ça te parle vraiment mais bon ^^

dit moi ce que tu comprend ^^

Skizo !
Un tien vaux mieux que deux tu l'auras !

jonjon

Skizo, yes c'est exactement avec ça que je me suis cassé les dents, merci je vais voir pour travailler avec ton code ce soir à la maison. J'avais déjà fait des tableaux comme ça mais impossible de les vérifier car je n'arrivai pas a les afficher avec des serial.prints.

Vince, je suis preneur de ta solution pour les mouvements simultanés si ça te dis de me laisser un bout de code ;).

Merci les mecs !

osaka

#7
Jun 06, 2012, 04:25 pm Last Edit: Jun 06, 2012, 05:20 pm by osaka Reason: 1

Vince, je suis preneur de ta solution pour les mouvements simultanés si ça te dis de me laisser un bout de code ;).


Pour mieux comprendre comment faire du "simultané" avec ton arduino, il faut comprendre et voir la fonction loop() comme une boucle infinie, en fait loop est une fonction appelé en boucle dans ton code arduino mais cette boucle malgré qu'elle soit "masquée" par l'ide arduino existe bien une fois le code compilés.

Donc c'est comme si tu fesais en réalité :

Code: [Select]

while(1)
{
 for(pos1 = pos1; pos1 <= pos1final; pos1 += 1) // en premier mvt servo1, pos1 -> pos1final
 for(pos2 = pos2; pos2 <= pos2final; pos2 += 2) // en second  mvt servo2, pos2 -> pos2final
 for(pos3 = pos3; pos3 <= pos3final; pos3 += 1) // en dernier mvt servo3,  pos3 -> pos3final
} // on repart au début de la boucle ...


Chaque mouvement est exécuté consécutivement 1 for{} à la fois et rebouclé constamment via loop ( while(1) ).
Si tu veux faire du quasi simultané (puisqu'on ne peux pas faire du multis taches), tu peux profité de l'inertie offerte par loop.

Code: [Select]

void loop() //on imagineras qu'il s'agit d'un while(1)
{
 if(pos1 <= 180) //temps qu'on a pas atteint les 180° ou autre valeur maximum demandé
 {
   myservo1.write(pos1)
   pos1+=1; //on incrément de la valeur désirée soit 1 ici à la fois à chaque tour de loop
 }
 else // on repart à 0 une fois les 180°ou autre atteint
 {
   pos1 = 0;
 }
 
//on viens d'incrémenté de 1 le premier servo, passons au suivant
 if(pos2 <= 180)
 {
   myservo2.write(pos2)
   pos2+=2; //pour celui ci nous incrémentons de 2
 }
 else
 {
   pos2 = 0;
 }
 
// au troisieme ...
 if(pos3 <= 180)
 {
   myservo3.write(pos3)
   pos3+=1;
 }
 else
 {
   pos3 = 0;
 }
}


On incrémentera d'une valeur mini chaque servo consécutivement et non de l'intégralité du mouvement de chaque servo en 1 seul fois à chaque tour de loop.
Maintenant si tu veux décrémenté après avoir atteint les 180 par exemple il suffira d'ajouter une condition de "sens" par exemple.

Code: [Select]

void loop() //on imagineras qu'il s'agit d'un while(1)
{
 if(sens == 1) //si dans le sens positif
 {
     if(pos1 <= 180) // temps qu'on a pas atteint les 180
     {
       pos1+=1; // on incrémente de 1
     }
     else // sinon une fois les 180
     {
       sens = 0; // on inverse le sens
     }
 }
 else //si dans le sens négatif
 {
     if(pos1 >= 0) // temps qu'on a pas atteint les 0
     {
       pos1-=1; //on décrémente de 1
     }
     else // sinon une fois le 0 atteint
     {
       sens = 1; // on inverse le sens
     }
 }
 myservo1.write(pos1) //on positionne suivant la valeur incrémenté ou décrémenté précédemment

} // on recommence les même actions avec les nouvelles valeurs ...


bon ça peux paraitre plus long, mais c'est une façon de faire du semi simultané il faut joué avec loop et procédé par étape.
Entre () c'est ainsi que fonctionne http://arduino.cc/en/Tutorial/BlinkWithoutDelay, on profite du fait que loop tourne en boucle infinie pour comparer deux temps (currentMillis - previousMillis) mesurer en deux tour de boucle consécutif.

vince3838

Salut,
voila un bout de code pour gerer 3 pap (mon appli), il faudra donc adapter pour des servos mais l'idee est la.
avec ça tes vitesses sont proportionnelles a la course a parcourir, tu as donc des trajectoires propres.

Code: [Select]
//soit 3 valeurs de destination : val0, val1 et val2
// soit 3 valeurs precedentes : prev0, prev1 et prev2
// axe 1:dirpin:3 steppin:2
// axe 2:dirpin:6 steppin:5
// axe 3:dirpin:9 steppin:8

long val0, val1, val2;
long prev0, prev1, prev2;
long d0,d1,d2;

//determination du sens
     if(val0-prev0>0){digitalWrite(3,HIGH);}else{digitalWrite(3,LOW);}
     if(val1-prev1>0){digitalWrite(6,HIGH);}else{digitalWrite(6,LOW);}
     if(val2-prev2>0){digitalWrite(9,HIGH);}else{digitalWrite(9,LOW);}

//calcul de la course a parcourir (en abs car on a deja le sens)
    d0 = val0 - prev0; 
    d1 = val1 - prev1;
    d2 = val2 - prev2; 

// identifier distance maxi pour les ratios
  long m= max(max(abs(d0),abs(d1)),abs(d2));

debut de calcul des ratios, le plus grand sera égal à 1 (distance maxi/distance maxi):
  float r0=(float)abs(d0) / (float) m;
  float r1=(float)abs(d1) / (float) m;
  float r2=(float)abs(d2) / (float) m;
     
  float a0=0,a1=0,a2=0;
 
   for(long n=0;n<m;n++) //debut boucle de mouvement
  {
    // on incrémente:
    a0=a0+r0;
    a1=a1+r1;
    a2=a2+r2;
    //si erreur >=1 on fait une step, puis on decremente l'erreur
    //les reels... juste pour info du nombre réel de steps effectué pour debug
    if(a0>=1.00){a0--;reel0++;digitalWrite(2, HIGH);digitalWrite(2, LOW);}
    if(a1>=1.00){a1--;reel1++;digitalWrite(5, HIGH);digitalWrite(5, LOW);}
    if(a2>=1.00){a2--;reel2++;digitalWrite(8, HIGH);digitalWrite(8, LOW);}   
    delayMicroseconds(vitesse); //vitesse globale des moteurs à définir suivant installation
  }
  // RAZ DELTA CAR MOUVEMENT EFFECTUE
    prev0 = val0;
    prev1 = val1;
    prev2 = val2;


voilou!

jonjon

Ok, j'ai donc fait un petit mixte de vos conseils et je m'en suis sortie avec mes calcules.
Par contre j'arrvie avec un nouveau problème, j'ai l'impression que la uno n'arrive pas gérer 3 servos en similtanés. Avant avec les 3 servos qui bougés l'un après l'autre il ni avait pas de problème, maintenant qu'ils bougent en +/- similtanés, il ni en a que deux qui sont pris en charge et des fois le troisieme fonctionne mais pas les deux premiers.

J'ai essayé sur les pins 3,4,5 et 9,10,11 avec la lib #include <Servo.h>.

vous en pensez quoi ?
J'ai lu qu'on peut utiliser la lib MegaServo ou modifier la lib Servo, mais j'ai pas reussi.

voila une partie du nouveau code :

Code: [Select]

for (count=0;count<=300;count++) {   //!!!!!!!!!!! REMPLACER LES 300 PARUN CHIFFRE EXACTE
   Serial.print("count="); Serial.println(count);

     if(pos1 <= pos1finalArray[0]) { pos1 = pos1 + r0; if(pos1 >= pos1finalArray[0]) { Serial.println("****fin -1- "); stop1=1; }}
     if(pos1 >= pos1finalArray[0]) { pos1 = pos1 - r0; if(pos1 <= pos1finalArray[0]) { Serial.println("****fin -1- "); stop1=1; }}
     myservo1.write(pos1);

     if(pos2 <= pos1finalArray[1]) { pos2 = pos2 + r1; if(pos2 >= pos1finalArray[1]) { Serial.println("****fin -2- "); stop2=1; }}
     if(pos2 >= pos1finalArray[1]) { pos2 = pos2 - r1; if(pos2 <= pos1finalArray[1]) { Serial.println("****fin -2- "); stop2=1; }}
     myservo2.write(pos2);

     if(pos3 <= pos1finalArray[2]) { pos3 = pos3 + r2;if(pos3 >= pos1finalArray[2]) { Serial.println("****fin -3- "); stop3=1; }}
     if(pos3 >= pos1finalArray[2]) { pos3 = pos3 - r2;if(pos3 <= pos1finalArray[2]) { Serial.println("****fin -3- "); stop3=1; }}
     myservo3.write(pos3);

      if (stop1 == 1  && stop2 == 1 && stop3 == 1) {
              Serial.println("****fin -ALL- ");
              count=300;
      }

}


vince3838

salut !

Je pense que tu devrais laisser un petit delay après avoir envoyé la commande a tes servos, et tu devrais utiliser la commande writeMicroseconds aussi, tu seras plus précis, surtout avec ce genre de calculs.

une petite question, ton "count" en entrée de boucle, ça correspond a ta distance max comme sur mon code?
tu pourrais poster tout le reste du code ce serait plus facile a comprendre.

et ne t'inquiete pas la UNO arrive parfaitement a gerer 3 servos ! (mon ancien delta etait comme ça).

tiens moi au courant .


skizoh

Salut, je suis d accord avec vinc, tu devrais laissé un petit delay mais ne fait pas un delay brut a la delay(1000); c'est bloquand et je suis pas sur que ça face avancé ton problème, fait une petit boucle vide ou tu mesure le temps écoulé et apres tu passe au servo suivant, allé tu touche au but et pour ce qui est de

Quote
J'avais déjà fait des tableaux comme ça mais impossible de les vérifier car je n'arrivai pas a les afficher avec des serial.prints.


c'est normal enfin j'ai pas vue ton code mais je me doute de ton erreurs, pour ça tu doit faire comme ça:

Code: [Select]

int i=0,tab[20];

while (i<20)

{

Serial.print(tab[i]); // tu dois précisé la case tab [ num_case ] , malheureusement tu ne peut pas imprimer le tableau d'un coup.

}




voila ! salut :)

Skizo !

Un tien vaux mieux que deux tu l'auras !

jonjon

Salut,

Mon count est indépendant, il me serait juste de variable pour stopper la boucle une fois que tout les servos sont positionnés (normalement +/- 100/150 count et +/- en même temps maintenant grâce au calcule des ratios ;) je dis que count=300 et la boucle s'arrête.

J'ai trouvé mon problème, effectivement la Uno fonctionne bien avec les trois servos en pin 2/3/4, j'avais un problème dans mon code avec des valeurs négatives, c'est réglé, mon delta fonctionne :)

Pour mes tableaux le print me sortait des caractères bizarre avant, mais avec un Serial.print("DEC"); ou Serial.print("BIN"); (je ne sais plus lequel, ça marche. Un problème de configuration de mon tableau au départ, les erreurs du débutant quoi ;)

Merci pour votre aide !!! je posterai un vidéo une fois le projet terminé, je dois bosser sur le look maintenant :=)

vince3838

OK,  rien de grave alors !! 
J'attends avec impatience tes vidéos, tu as des bras de quelle longueur? (bras moteurs et bras secondaires?) juste pour avoir une idée de la taille de la bête? et tu as quoi au bout? pince, ventouse?




skizoh

Quote
Un problème de configuration de mon tableau au départ


lol non, tu ne configure pas tes tableaux, problème d'affichage tout simplement, en BIN = binaire ou DEC = Décimal ou HEX = Hexadécimal tu précise le type ça résous parfois les erreurs.

Skizo !
Un tien vaux mieux que deux tu l'auras !

Go Up