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.
/*
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
}