182 réponses à ce sujet

[Oracid]

Et bien, non ! Rien ne va plus !

Je pensais que cela fonctionnait parce que le UNO était alimenté par l'USB.

Si j'enlève l'USB et le cavalier, le UNO n'est plus alimenté.

[Oracid]

En définitive, il faut alimenter spécifiquement le UNO si le cavalier est enlevé.

Je vais piquer la tension sur un connecteur servo et la souder sur le point de soudure du Vin.

Malheureusement, ce Shield ne redistribue pas les connecteurs du UNO en mode gigogne.

[Oracid]

Et voilà, le travail !

Comme ça, tout fonctionne.

 

[Oracid]

J'ai donc testé les modifications de Sandro. Dans un premier temps, directement sur mon Quad qui n'a pas du tout apprécié et un servo a rendu l'âme.
J'ai donc monté un banc de test spécifique avec 2 servos MG90S. Ce banc de test comporte une seule patte, la patte avant droite (FR).
Le servo de gauche est le FRLS, et celui de droite est le FRRS.
 
Le 4 barres dessine inlassablement une ligne verticale sur l'axe Y, avec x=0 et y=-50 à y=50.
Cela fonctionne bien, mais bizarrement ! Par exemple, si je commentarise la ligne 107, cela ne fonctionne plus.
 
Quelques questions:
1 - pourquoi à la ligne 36, la position initiale est à 1700 ?
2 - pourquoi à la ligne 38 et 39, les min et max sont à 1000 et 2000, au lieu de 500 et 2500 ?
3- à partir de la 70 à 80, j'essaye de positionner les servos dans leur position initiale que j'ai défini à 50° et 180°-50°, mais cela ne fonctionne pas.
 
Voici le code et le banc de test.
 

Spoiler

// FourBar525-Sandro-V2 n'utilise pas la bibli Servo.h - 07/12/2020
//équivalences pin/port
//  2   PD2
//  3   PD3
//  4   PD4
//  5   PD5
//  6   PD6
//  7   PD7
//  8   PB0
//  9   PB1

//octets avec un 0 à l'endroit correspondant à un pin dans les registres et 1 ailleurs
#define ZERO_DU_PIN_2 B11111011
#define ZERO_DU_PIN_3 B11110111
#define ZERO_DU_PIN_4 B11101111
#define ZERO_DU_PIN_5 B11011111
#define ZERO_DU_PIN_6 B10111111
#define ZERO_DU_PIN_7 B01111111
#define ZERO_DU_PIN_8 B11111110
#define ZERO_DU_PIN_9 B11111101

//nom du pot correspondant à chaque pin
#define PORT_DE_PIN_2 PORTD
#define PORT_DE_PIN_3 PORTD
#define PORT_DE_PIN_4 PORTD
#define PORT_DE_PIN_5 PORTD
#define PORT_DE_PIN_6 PORTD
#define PORT_DE_PIN_7 PORTD
#define PORT_DE_PIN_8 PORTB
#define PORT_DE_PIN_9 PORTB

#define PERIODE_US 20000  //période

int i=0;
unsigned long start_time;
uint16_t duree_pulse_us[10]={0,0,1700, 1700, 1700, 1700, 1700, 1700, 1700, 1700};   //position initiale

uint16_t duree_pulse_us_min[10]={0,0,1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000};
uint16_t duree_pulse_us_max[10]={0,0,2000, 2000, 2000, 2000, 2000, 2000, 2000, 2000};
int angle_min[10]={0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
int angle_max[10]={0, 0, 180, 180, 180, 180, 180, 180, 180, 180};

const int nb=10;                         // Number of servos
int Srv[nb];                          // Servos table 
int OnOff[nb]={0,0,1,1,0,0,0,0,0,0};        // Servos table on/off

int FRLS=2, FRRS=3, FLLS=4, FLRS=5, BRLS=6, BRRS=7, BLLS=8, BLRS=9;
//***************** Correction for 50° LS and RS servos position     **************************************
// servos         FRLS  FRRS  FLLS  FLRS  BRLS  BRRS  BLLS  BLRS modify these values according to your servos
int Err[nb]={0,0,   0,    2,    5,    5,    4,    7,    12,   2    };  
//*********************************************************************************************************

int FRx[]= {  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,    0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0};
int FLx[]= {  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,    0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0};
int BRx[]= {  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,    0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0};
int BLx[]= {  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,    0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0};

int FRy[]= {-50,-40,-30,-20,-10,  0, 10, 20, 30, 40, 50,   50, 40, 30, 20, 10,  0,-10,-20,-30,-40,-50};
int FLy[]= {-50,-40,-30,-20,-10,  0, 10, 20, 30, 40, 50,   50, 40, 30, 20, 10,  0,-10,-20,-30,-40,-50};
int BRy[]= {-50,-40,-30,-20,-10,  0, 10, 20, 30, 40, 50,   50, 40, 30, 20, 10,  0,-10,-20,-30,-40,-50};
int BLy[]= {-50,-40,-30,-20,-10,  0, 10, 20, 30, 40, 50,   50, 40, 30, 20, 10,  0,-10,-20,-30,-40,-50};

int lgTab = sizeof(FRx)/sizeof(int);
         
void  setup() {
  pinMode(0,INPUT_PULLUP);                // start/reset button attachment
  Serial.begin(9600);
  for(int i=2;i<=9;i++)  {  pinMode(i,OUTPUT);  }

  // Servos angle initialization
  int a=50;
  if(OnOff[FRLS]) set_position_degrees(Srv[FRLS],    a+Err[FRLS]);  // FR Front Right leg - LS Left   Servo
  if(OnOff[FRRS]) set_position_degrees(Srv[FRRS],180-a+Err[FRRS]);  // FR Front Right leg - RS Right  Servo
  if(OnOff[FLLS]) set_position_degrees(Srv[FLLS],    a+Err[FLLS]);  // FL Front Left  leg - LS Left   Servo
  if(OnOff[FLRS]) set_position_degrees(Srv[FLRS],180-a+Err[FLRS]);  // FL Front Left  leg - RS Right  Servo
  if(OnOff[BRLS]) set_position_degrees(Srv[BRLS],    a+Err[BRLS]);  // BR Back  Right leg - LS Left   Servo
  if(OnOff[BRRS]) set_position_degrees(Srv[BRRS],180-a+Err[BRRS]);  // BR Back  Right leg - RS Right  Servo
  if(OnOff[BLLS]) set_position_degrees(Srv[BLLS],    a+Err[BLLS]);  // BL Back  Left  leg - LS Left   Servo
  if(OnOff[BLRS]) set_position_degrees(Srv[BLRS],180-a+Err[BLRS]);  // BL Back  Left  leg - RS Right  Servo
  envoyer_pulse();
  
  Serial.print("\n\t To start, click on the Start button");
  while( digitalRead(0) );  delay(400);       // waiting for start button pressed 
  Serial.print("\n\t Started");
}

void loop() {
unsigned long duration=micros()-start_time;
  envoyer_pulse();
  calculer_nouvelle_position(i);
  i++;

  //attendre la fin de la période
  while(duration < PERIODE_US)  {}
}

void calculer_nouvelle_position(int index){
//    Serial.print("\n\t calculer_nouvelle_position()");
    int i=index % lgTab; //index croit indéfiniment, i correspond à l'index dans la période
    InverseKinematics(FRx[i],FRy[i],FRLS,FRRS);
    InverseKinematics(BLx[i],BLy[i],BLLS,BLRS);
    InverseKinematics(FLx[i],FLy[i],FLLS,FLRS);
    InverseKinematics(BRx[i],BRy[i],BRLS,BRRS);
}

void InverseKinematics(int Px, int Py, int LS, int RS){
  Serial.print("\n\t InverseKinematics()");
  const float A1x=0, A1y=140, A2x=0, A2y=140;                   // Values of servos positions
  const float a1=112, c1=80, a2=112, c2=80;                     // Values of leg sizes lengths

  float d=A1y-Py, e=Px;                                   // Calculation of inverse kinematics
  float b=sqrt((d*d)+(e*e));                              // Calculation of inverse kinematics
  float S=acos(d/b);  if(e<0)S=(-S);                      // Calculation of inverse kinematics
  float A12=acos(((b*b)+(c1*c1)-(a1*a1))/(2*b*c1));       // Calculation of inverse kinematics
  float A22=acos(((b*b)+(c2*c2)-(a2*a2))/(2*b*c2));       // Calculation of inverse kinematics
  float A11=(PI/2)-A12+S;                                 // Calculation of inverse kinematics
  float A21=(PI/2)-A22-S;                                 // Calculation of inverse kinematics
  int S1=round(A11*57.296);                               // left servo angle in degree
  int S2=round(180-(A21*57.296));                         // right servo angle in degree

// DEBUG
//  Serial.print("\n\t Position to reach : Px=");Serial.print(Px);Serial.print("  Py=");Serial.print(Py);
//  Serial.print("\n\t d=");Serial.print(d);Serial.print("\t\t e=");Serial.print(e);
//  Serial.print("\t\t b=");Serial.print(b);Serial.print("\t\t S=");Serial.print(S*57.296);
//  Serial.print("\n\t A11=");Serial.print(A11*57.296);Serial.print("\t\t A12=");Serial.print(A12*57.296);
//  Serial.print("\t\t A22=");Serial.print(A22*57.296);Serial.print("\t\t A21=");Serial.print(A21*57.296);
//  Serial.print("\n\t Result of calculations, angles of the servos");
//  Serial.print("\n\t S1=");Serial.print(S1);Serial.print("°\t\t\t S2=");Serial.print(S2);Serial.print("°");

  if ( b>(a1+c1) ){
    Serial.print("\n\t Target point Px=");Serial.print(Px);Serial.print(" Py=");Serial.print(Py);Serial.print("\t b=");Serial.print(b);Serial.print(" > ");
    Serial.print(a1+c1);Serial.print(" is too long. Target impossible to reach   !!!!!");
    return;
  }
  if (S1+Err[LS]<0){
    Serial.print("\n\t Position to reach : Px=");Serial.print(Px);Serial.print("  Py=");Serial.print(Py);Serial.print("\t angle S1<0° is not reachable   !!!!!");
    return;
  }
  if (S2+Err[RS]>180){
    Serial.print("\n\t Position to reach : Px=");Serial.print(Px);Serial.print("  Py=");Serial.print(Py);Serial.print("\t angle S2<0° is not reachable   !!!!!");
    return;
  }
  if (S1+Err[LS]>120){
    Serial.print("\n\t Position to reach : Px=");Serial.print(Px);Serial.print("  Py=");Serial.print(Py);Serial.print("\t angle S1>140° is not reachable   !!!!!");
    return;
  }
  if (S2+Err[RS]<60){
    Serial.print("\n\t Position to reach : Px=");Serial.print(Px);Serial.print("  Py=");Serial.print(Py);Serial.print("\t angle S2<40° is not reachable   !!!!!");
    return;
  }
//  Serial.print("\t executed command");
  if (OnOff[LS]) set_position_degrees(LS,S1+Err[LS]);   // set target Left servo position if servo switch is On
  if (OnOff[RS]) set_position_degrees(RS,S2+Err[RS]);   // set target Right servo position if servo switch is On
}

void envoyer_pulse(){
//  Serial.print("\n\t envoyer_pulse()");
  start_time=micros();
  //mise à 1 de tous les pins
  PORTD |= B11111100; //mise à 1 des pins 2 à 7 du port D (ie pins 2 à 7 de l'arduino)
  PORTB |= B00000011; //mise à 1 des pins 0à 1 du port B (ie pins 8 et 9 de l'arduino)
  
  while( (PORTD & 0B11111100) | (PORTB & 0B00000011) )  //tant qu'au moins un des pins est à 1
  {
    unsigned long duree_actuelle_du_pulse=micros()-start_time;  //durée écoulée depuis le début du pulse
    if(duree_actuelle_du_pulse>=duree_pulse_us[2])      PORT_DE_PIN_2 &= ZERO_DU_PIN_2;
    if(duree_actuelle_du_pulse>=duree_pulse_us[3])      PORT_DE_PIN_3 &= ZERO_DU_PIN_3;
    if(duree_actuelle_du_pulse>=duree_pulse_us[4])      PORT_DE_PIN_4 &= ZERO_DU_PIN_4;
    if(duree_actuelle_du_pulse>=duree_pulse_us[5])      PORT_DE_PIN_5 &= ZERO_DU_PIN_5;
    if(duree_actuelle_du_pulse>=duree_pulse_us[6])      PORT_DE_PIN_6 &= ZERO_DU_PIN_6;
    if(duree_actuelle_du_pulse>=duree_pulse_us[7])      PORT_DE_PIN_7 &= ZERO_DU_PIN_7;
    if(duree_actuelle_du_pulse>=duree_pulse_us[8])      PORT_DE_PIN_8 &= ZERO_DU_PIN_8;
    if(duree_actuelle_du_pulse>=duree_pulse_us[9])      PORT_DE_PIN_8 &= ZERO_DU_PIN_9;
  }
}

void set_position_degrees(byte pin, int angle_deg){
//  Serial.print("\n\t set_position_degrees()");
  uint16_t duree_us=map(angle_deg, angle_min[pin], angle_max[pin], duree_pulse_us_min[pin], duree_pulse_us_max[pin]);
  duree_pulse_us[pin]=duree_us;
}

 

                                                                     

[Sandro]

Bonsoir,

tout d'abord, je suis désolé si la destruction du servo vient de mon code. Est-ce que tu as une idée de ce qui peut l'avoir cassé? Appui contre butée? Surchauffe? Tu alimentes tes servos avec quelle tension vs quelle tension nominale?

 

Pour répondre à tes questions :

1) la valeur de 1700 est une valeur "au hasard". Il me semble que j'avais initialement mis 1500 ("le milieu"), puis changé à 1700 pour faire bouger un peu les servos. Tu peux sans problème changer cette valeur pour 1500, ou une valeur de ton choix pour chaque servo. Tu peux aussi appeller set_position_degrees depuis le setup pour fixer la position initiale (qui sera exécutée au premier appel de envoyer_pulse)

 

2) J'avais en tête que les impulsions des servos allaient de 1000 à 2000µs. Mais je me trompe ou que tes servos exploitent une gamme plus large, libre a toi de modifier (j'ai volontairement mis un tableau de constantes pour chaque servo dans le but de facilement pouvoir configurer : dans le cas contraire, j'aurais juste pu utiliser une valeur fixe en dur dans le code)

 

3) c'est normal : mon code, contrairement à la librarie servo.h n'utilise pas interruptions : chaque appel à envoyer_pulse génère une seule et unique impulsion. Il faut donc appeller la fonction envoyer_pulse toutes les PERIODE_US microssecondes (ie toutes mes 20 000µs si on veut respecter la convention standard des servos de modélisme, ou on peut tenter une période moindre si on veut essayer d'augmenter la fréquence d'asservissement).

 

Donc pour que ça marche, il faut impérativement une boucle semblable au while (nb : le calcul de la nouvelle position est facultatif : par défaut on garde la dernière position demandée).

 

 

 

 

Je vois également que tu as tenté de faire quelques modifications sur la structure du code, qui, j'en ai bien peur, ne le rendent probablement inopérant. Le fait que j'ai eut la "mauvaise" idée d'avoir deux variables locales avec le même nom dans mon code (start_time), a probablement du t'induire en erreur (en pratique, il s'agit de deux variables locales de portée ("visibilité") totalement disjointe, et qui n'ont aucune relation entre elles.

Sans compter que mon code est très "spécialisé", et est prévu pour fonctionner d'une manière très précise (et ne fonctionnera plus si certains détails changent). En somme, mon code n'a rien d'une libraire conçue pour être utilisée facilement au gré de l'utilisateur : il s'agit au contraire d'un canevas qui "force" l'utilisateur à l'utiliser d'une manière très précise.

 

Voici donc une version un peu clarifiée, spécifiant explicitement ce que l'utilisateur peut faire ou ne pas faire :

Spoiler

/******************************
 * SECTION 1 : PLACEZ ICI VOS INCLUDES, DEFINES ET VARIABLES GLOBALES
 *****************************/





/******************************
 * SECTION 2 : NE PAS MODIFIER (DEFINES DES PORTS), sauf si vous volez utiliser d'autre pins que 2 à 9 et si vous savez ce que vous faites (implique beaucoup d'autres modifications plus loin dans le code)
 ******************************/

//équivalences pin/port
//  2   PD2
//  3   PD3
//  4   PD4
//  5   PD5
//  6   PD6
//  7   PD7
//  8   PB0
//  9   PB1

//octets avec un 0 à l'endroit correspondant à un pin dans les registres et 1 ailleurs
#define ZERO_DU_PIN_2 B11111011
#define ZERO_DU_PIN_3 B11110111
#define ZERO_DU_PIN_4 B11101111
#define ZERO_DU_PIN_5 B11011111
#define ZERO_DU_PIN_6 B10111111
#define ZERO_DU_PIN_7 B01111111
#define ZERO_DU_PIN_8 B11111110
#define ZERO_DU_PIN_9 B11111101

//nom du pot correspondant à chaque pin
#define PORT_DE_PIN_2 PORTD
#define PORT_DE_PIN_3 PORTD
#define PORT_DE_PIN_4 PORTD
#define PORT_DE_PIN_5 PORTD
#define PORT_DE_PIN_6 PORTD
#define PORT_DE_PIN_7 PORTD
#define PORT_DE_PIN_8 PORTB
#define PORT_DE_PIN_9 PORTB

/******************************
 * SECTION 3 : VALEURS À MODIFIER : PARAMÈTRES DE LA COMMANDE DES SERVOS
 * il faut addapter ces valeurs à vos servos.
 * la position i du tableau correspond au pin i. Les positions 0 et 1 sont ignorées (car on n'utilise pas les pins 0 et 1)
 *****************************/
 
#define PERIODE_US 20000  // PÉRIODE DU PWM, EN MICROSECONDES : valeur "par défaut" de 20000µs=20ms : sur la plupart des servos cette durée peut être réduite, pour certains qu'un peu, pour d'autres quasiment jusqu'à la longueur de l'impulsion la plus longue. NB : cette durée correspon également à la période d'asservissement (ie à quelle période on vas obtenir une nouvelle position pour le servo)

uint16_t duree_pulse_us[10]={0,0,1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500};   //POSITION INITIALE, en durée de pulse en µs. NB : cette position n'a d'influance que si on appelle envoyer_pulse un certain nombre de fois sans avoir auparavent spécifié de position avec set_position_degrees

uint16_t duree_pulse_us_min[10]={0,0,1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000}; //DURÉE MINIMALE D'UN PULSE : à adapter selon le moteur choisit (1000 pour certains, un peu moins pour d'autres, peut potentiellement varier d'un servo à l'autre). NB : cette valeur peut être supérieure min atteignable, dans ce cas le moteur n'irra simplement pas jusqu'au bout de sa course
uint16_t duree_pulse_us_max[10]={0,0,2000, 2000, 2000, 2000, 2000, 2000, 2000, 2000}; //DURÉE MAXIMALE D'UN PULSE : idem
int angle_min[10]={0,0,0, 0, 0, 0, 0, 0, 0, 0};   //ANGLE CORRESPONDANT À LA DURÉE_PULSE_US_MIN
int angle_max[10]={0,0,180, 180, 180, 180, 180, 180, 180, 180};   //ANGLE CORRESPONDANT À LA DURÉE_PULSE_US_MIN


/******************************
 * SECTION 4 :  NE PAS MODIFIER (DÉFINITION DES FONCTIONS INTERNES)
 *****************************/
void envoyer_pulse()
{
  unsigned long start_time=micros();
  //mise à 1 de tous les pins
  PORTD |= B11111100; //mise à 1 des pins 2 à 7 du port D (ie pins 2 à 7 de l'arduino)
  PORTB |= B00000011; //mise à 1 des pins 0à 1 du port B (ie pins 8 et 9 de l'arduino)
  
  while( (PORTD & 0B11111100) | (PORTB & 0B00000011) )  //tant qu'au moins un des pins est à 1
  {
    unsigned long duree_actuelle_du_pulse=micros()-start_time;  //durée écoulée depuis le début du pulse
    if(duree_actuelle_du_pulse>=duree_pulse_us[2])
      PORT_DE_PIN_2 &= ZERO_DU_PIN_2;

    if(duree_actuelle_du_pulse>=duree_pulse_us[3])
      PORT_DE_PIN_3 &= ZERO_DU_PIN_3;
      
    if(duree_actuelle_du_pulse>=duree_pulse_us[4])
      PORT_DE_PIN_4 &= ZERO_DU_PIN_4;

    if(duree_actuelle_du_pulse>=duree_pulse_us[5])
      PORT_DE_PIN_5 &= ZERO_DU_PIN_5;

    if(duree_actuelle_du_pulse>=duree_pulse_us[6])
      PORT_DE_PIN_6 &= ZERO_DU_PIN_6;

    if(duree_actuelle_du_pulse>=duree_pulse_us[7])
      PORT_DE_PIN_7 &= ZERO_DU_PIN_7;

    if(duree_actuelle_du_pulse>=duree_pulse_us[8])
      PORT_DE_PIN_8 &= ZERO_DU_PIN_8;

    if(duree_actuelle_du_pulse>=duree_pulse_us[9])
      PORT_DE_PIN_8 &= ZERO_DU_PIN_9;
  }
}

void set_position_degrees(byte pin, int angle_deg)
{
  uint16_t duree_us=map(angle_deg, angle_min[pin], angle_max[pin], duree_pulse_us_min[pin], duree_pulse_us_max[pin]);
  duree_pulse_us[pin]=duree_us;
}


/******************************
 * SECTION 5 : PLACEZ ICI CE QUE VOUS VOULEZ (variables globales, définition de fonctions, définition de classes, ...)
 *****************************/





/******************************
 * SECTION 6 : À COMPLÉTER : CALCUL DE LA NOUVELLE POSITION
 * c'est le bout de code utilisateur qui sera appellé durant le "loop". Il ne faut mettre aucun code ailleurs (sauf code de setup). NB : il est par contre possible de créer des fonctions (à mettre en section 5) qui peuvent être appellées depuis la fonction calculer_nouvelle_position
 * NB : il est AUTORISÉ de mettre à jour une/des positions de servos depuis ce code, mais pas obligatoire (s'il n'y a pas de nouvelle position donnée, on conserve l'ancienne).
 * Toutes les nouvelles positions données ici entreront en appliquation simultanément, au prochain pulse du PWM
 * ATTENTION : le temps d'exécution de cette fonction ne doit pas déppasser PERIODE_US - max(duree_pulse_us_max) - "une petite marge"
 * 
 * rq : la variable index s'incrémente à chaque appel, ce qui permet facilement de parcourir une liste de positions prédéfinies. Il n'est pas obligatoire de s'en servir (on peut par exemple gérer soit même à base du temps)
 *****************************/
void calculer_nouvelle_position(uint32_t index){
  //int taille_tableau=42
  //int i=index%taille_tableau
  //décomenter (et addapter) les deux lignes ci-dessus si on veut avoir un index i qui vas de 0 à taille_tableau-1

  // à compléter
}



/******************************
 * SECTION 7 : SETUP : MODIFIER UNIQUEMENT LÀ OÙ C'EST SUGGÉRER
 *****************************/

void setup() {


  //ICI, CE QUE VOUS VOULEZ

  //NE PAS MODIFIER D'ICI
  for(int i=2;i<=9;i++)
  {
    pinMode(i,OUTPUT);
  }
  // À ICI


  //ICI CE QUE VOUS VOULEZ

  //FACULTATIF : SPÉCIFIER UNE POSITION INITIALE EN DEGRÉS (remplace la valeur données dans duree_pulse_us en section 3). Décommenter pour utiliser
  /*set_position_degrees(2, 90);  //Rremplacer 90 par une valeur ou une formule de votre choix
  set_position_degrees(3, 90);  //Rremplacer 90 par une valeur ou une formule de votre choix
  set_position_degrees(4, 90);  //Rremplacer 90 par une valeur ou une formule de votre choix
  set_position_degrees(5, 90);  //Rremplacer 90 par une valeur ou une formule de votre choix
  set_position_degrees(6, 90);  //Rremplacer 90 par une valeur ou une formule de votre choix
  set_position_degrees(7, 90);  //Rremplacer 90 par une valeur ou une formule de votre choix
  set_position_degrees(9, 90);  //Rremplacer 90 par une valeur ou une formule de votre choix
  set_position_degrees(10, 90);  //Rremplacer 90 par une valeur ou une formule de votre choix*/

  //ICI CE QUE VOUS VOULEZ

  //FACULTATIF : PLACER LES SERVOS EN POSITION INITIALE : Décommenter une des alternatives pour l'utiliser
  // ALTERNATIVE 1 : ATTENTE D'UN TEMPS FIXE : si utilisé, ne pas modifier les lignes ci-dessous excepté la valeur de duree_initialisation_en_secondes.
  /*float duree_initialisation_en_secondes=1.0;   //remplacer 1.0 par la durée en seconde nécessaire pour être sur que les servos ont le temps d'arriver à la position initiale souhaitée
  for(int i=0; i<duree_initialisation_en_secondes*1000000/PERIODE_US; i++)
  {
    unsigned long start_time_period=micros();
    envoyer_pulse();
    while(micros()-start_time_period<PERIODE_US)
    {
      //ne rien faire
    }
  }*/
  // ALTERNATIVE 2 : ATTENTE JUSQU'À APPUIT D'UN BOUTON SUR UN PIN : si utilisé, ne pas modifier les lignes ci-dessous excepté le choix du pin
  /*byte pin=A0;  //bouton sur pin A0
  bool attente_depart= digitalRead(pin);
  while(attente_depart) // waiting for start button pressed
  {
    unsigned long start_time_period=micros();
    envoyer_pulse();
    while(micros()-start_time_period<PERIODE_US)
    {
      if(digitalRead(pin))
        attente_depart= true;
    }
  }*/


  //ICI CE QUE VOUS VOULEZ
  
}





/******************************
 * SECTION 8 (LOOP) : NE PAS MODIFIER !!!
 *****************************/
uint32_t c=0;

void loop() {
  c++;
  unsigned long start_time_period=micros();
  envoyer_pulse();
  
  calculer_nouvelle_position(c); //nb : tout votre code sera exécuté depuis l'intérieur de cette fonction

  //attendre la fin de la période
  while(micros()-start_time_period<PERIODE_US)
  {
    //ne rien faire
  }
}

/******************************
 * SECTION 9 : POSSIBILITÉ DE DÉCLARER DES FONCTIONS CI DESSOUS
 *****************************/

 

Je te suggère d'essayer d'insérer ton code dans le canevas ci-dessous (qui explique mieux quoi faire ou ne pas faire).

STP : ne modifie rien en dehors de là où c'est explicitement indiqué : tu risque fortement de rendre le programme non fonctionnel (sauf si tu sais exactement ce que tu fais).

Si tu as besoin d'aide, c'est avec plaisir.

 

PS : dans le canevas, j'ai remplacé le pin 0 par le pin A0 pour le bouton, car utiliser les pins 0 et 1 est souvent une mauvaise idée vu qu'ils servent pur la communication en USB avec l'ordianteur

 

Fichier(s) joint(s)

•  sync_servo_v3.ino   8,75 Ko   111 téléchargement(s)

[Oracid]

Merci Sandro. Pas de souci pour le servo, j'en ai d'autres.

 

1 - Ok.

2 - Désolé, j'ai dit n'importe quoi ! Oui, 1000 à 2000us, c'est bon !

3 - La position initiale, c'est celle qui met le Quad debout, prêt à partir au moment où j'appuie sur le bouton Start. 

 

Non, non, je n'ai pas touché à la structure de ton code. J'ai un peu customisé, pour gagner des lignes, et j'ai mis tes fonctions à la fin, plutôt qu'au début.

Je ne me rappelle pas avoir touché à la variable start_time.

Ce qui m'inquiète le plus, c'est la ligne 107. C'est le Serial.print, première ligne de la fonction  InverseKinematics(), si je la mets en commentaire, plus rien ne fonctionne.

 

Pour le pin 0, vieux débat ! En pratique, cela n'a jamais eu aucune incidence.

 

Peu importe, je vais revoir tout ça.

[Sandro]

Bonjour,

pourtant, si tu regardes le code que j'avais posté pour la V2 (https://www.robot-ma...e-7#entry111960), j'ai bien 2 variables locales appelées start_time, et non une unique variable globale. Et la valeur de start_time est mise à micros en chaque début de boucle.

Mais après réflexion, je crois que ce point ne pose pas de problème significatif.

 

Le vrai problème vient je crois du fait d'avoir remplacé

  //attendre la fin de la période
  while(micros()-start_time<PERIODE_US)
  {
    //ne rien faire
  }

par

//attendre la fin de la période
  while(duration < PERIODE_US)  {}

En effet, ta variante est une boucle infinie, car rien ne vient mettre à jour duration.
 

[Oracid]

Oui, tu as raison !

Mais pourquoi ai-je fait cette modif ? C'est incroyable !

[Sandro]

Probablement car je calculais moi même "duration" dans le loop, avec la même formule, mais juste pour l'afficher.

Je suppose que tu as donc voulu supprimer le doublon en réutilisant duration (ce qui aurait été justifié si la formule ne changeait pas de valeur "toute seule")

[Oracid]

Tout va bien ! J'ai réussi à faire fonctionner le programme de Sandro.

Après l'avoir testé, je n'ai pas résisté à l'envie de le mettre à ma sauce. Je préfère un code concis, je n'aime pas dérouler des centaines de lignes.

 

Les principales modifications sont :

- j'ai gardé le setup() et le loop() au début, et j'ai déplacé les fonctions vers le bas.

- j'ai réécrit l'initialisation des servos à ma sauce. Cela fonctionne très bien.

- dans le loop(), la variable c était incrémentée en première ligne, je l'ai déplacée à la fin, car on manquait la première valeur du tableau.

- le plus important est sans aucun doute la modification des valeurs des tableaux duree_pulse_us_min[10] et duree_pulse_us_max[10] que j'ai passé de 1000 et 2000 à 500 et 2500. Avec les anciennes valeurs, les angles des servos étaient incorrectes.

Dans cette page,  https://www.arduino....s/servo/attach/ , on parle de valeurs de pulse allant de 544 à 2400. J'avais cela en tête, même si mes valeurs ne sont pas tout à fait identiques.

 

Voilà le résultat du mouvement du chapeau chinois. Ne vous méprenez pas, c'est pas mal pour un assemblage de pièces qui est très imprécis dans ce montage. Plus bas, j'ai mis l'image en référence.

 

 

 

Voici mon code.

Spoiler

// Version Sandro V3 - 19/12/2020
// Remarques : cela fonctionne très bien.
 
//équivalences pin/port
//  2   PD2
//  3   PD3
//  4   PD4
//  5   PD5
//  6   PD6
//  7   PD7
//  8   PB0
//  9   PB1
 
//octets avec un 0 à l'endroit correspondant à un pin dans les registres et 1 ailleurs
#define ZERO_DU_PIN_2 B11111011
#define ZERO_DU_PIN_3 B11110111
#define ZERO_DU_PIN_4 B11101111
#define ZERO_DU_PIN_5 B11011111
#define ZERO_DU_PIN_6 B10111111
#define ZERO_DU_PIN_7 B01111111
#define ZERO_DU_PIN_8 B11111110
#define ZERO_DU_PIN_9 B11111101
 
//nom du pot correspondant à chaque pin
#define PORT_DE_PIN_2 PORTD
#define PORT_DE_PIN_3 PORTD
#define PORT_DE_PIN_4 PORTD
#define PORT_DE_PIN_5 PORTD
#define PORT_DE_PIN_6 PORTD
#define PORT_DE_PIN_7 PORTD
#define PORT_DE_PIN_8 PORTB
#define PORT_DE_PIN_9 PORTB
 
#define PERIODE_US 20000  // PÉRIODE DU PWM, EN MICROSECONDES : valeur "par défaut" de 20000µs=20ms : sur la plupart des servos cette durée peut être réduite, pour certains qu'un peu, pour d'autres quasiment jusqu'à la longueur de l'impulsion la plus longue. NB : cette durée correspon également à la période d'asservissement (ie à quelle période on vas obtenir une nouvelle position pour le servo)
 
uint16_t duree_pulse_us[10]={0,0,1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500};   //POSITION INITIALE, en durée de pulse en µs. NB : cette position n'a d'influance que si on appelle envoyer_pulse un certain nombre de fois sans avoir auparavent spécifié de position avec set_position_degrees
 
//uint16_t duree_pulse_us_min[10]={0,0,1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000}; //DURÉE MINIMALE D'UN PULSE : à adapter selon le moteur choisit (1000 pour certains, un peu moins pour d'autres, peut potentiellement varier d'un servo à l'autre). NB : cette valeur peut être supérieure min atteignable, dans ce cas le moteur n'irra simplement pas jusqu'au bout de sa course
//uint16_t duree_pulse_us_max[10]={0,0,2000, 2000, 2000, 2000, 2000, 2000, 2000, 2000}; //DURÉE MAXIMALE D'UN PULSE : idem
uint16_t duree_pulse_us_min[10]={0,0, 500,  500,  500,  500,  500,  500,  500,  500}; //DURÉE MINIMALE D'UN PULSE : à adapter selon le moteur choisit (1000 pour certains, un peu moins pour d'autres, peut potentiellement varier d'un servo à l'autre). NB : cette valeur peut être supérieure min atteignable, dans ce cas le moteur n'irra simplement pas jusqu'au bout de sa course
uint16_t duree_pulse_us_max[10]={0,0,2500, 2500, 2500, 2500, 2500, 2500, 2500, 2500}; //DURÉE MAXIMALE D'UN PULSE : idem
int angle_min[10]={0,0,0, 0, 0, 0, 0, 0, 0, 0};   //ANGLE CORRESPONDANT À LA DURÉE_PULSE_US_MIN
int angle_max[10]={0,0,180, 180, 180, 180, 180, 180, 180, 180};   //ANGLE CORRESPONDANT À LA DURÉE_PULSE_US_MIN
 
const int nb=10;                         // Number of servos
int Srv[nb];                          // Servos table 
int OnOff[nb]={0,0,1,1,0,0,0,0,0,0};        // Servos table on/off
int FRLS=2, FRRS=3, FLLS=4, FLRS=5, BRLS=6, BRRS=7, BLLS=8, BLRS=9;
//***************** Correction for 50° LS and RS servos position     **************************************
// servos         FRLS  FRRS  FLLS  FLRS  BRLS  BRRS  BLLS  BLRS modify these values according to your servos
int Err[nb]={0,0,  11,  -10,    0,    0,    0,    0,    0,   0    };  
//*********************************************************************************************************
 
// 30/11/2020 - symetric Chapeau Chinois V11-1 - Ay=150, a1=112, c1=88, Speed=500, 400cm
// 29/11/2020 - symetric Chapeau Chinois V11-1 - Ay=150, a1=104, c1=88, Speed=500, 360cm
int FRx[]= { 80, 75, 70, 65, 60, 55, 50, 45, 40, 35, 30, 25, 20, 15, 10,  5,  0, -5,-10,-15,-20,-25,-30,-35,-40,-45,-50,-55,-60,-65,-70,-75,-80,    -80,-75,-70,-65,-60,-55,-50,-45,-40,-35,-30,-25,-20,-15,-10, -5,  0,  5, 10, 15, 20, 25, 30, 35, 40, 45, 50, 55, 60, 65, 70, 75, 80};
int FLx[]= { 80, 75, 70, 65, 60, 55, 50, 45, 40, 35, 30, 25, 20, 15, 10,  5,  0, -5,-10,-15,-20,-25,-30,-35,-40,-45,-50,-55,-60,-65,-70,-75,-80,    -80,-75,-70,-65,-60,-55,-50,-45,-40,-35,-30,-25,-20,-15,-10, -5,  0,  5, 10, 15, 20, 25, 30, 35, 40, 45, 50, 55, 60, 65, 70, 75, 80};
int BRx[]= {-80,-75,-70,-65,-60,-55,-50,-45,-40,-35,-30,-25,-20,-15,-10, -5,  0,  5, 10, 15, 20, 25, 30, 35, 40, 45, 50, 55, 60, 65, 70, 75, 80,     80, 75, 70, 65, 60, 55, 50, 45, 40, 35, 30, 25, 20, 15, 10,  5,  0, -5,-10,-15,-20,-25,-30,-35,-40,-45,-50,-55,-60,-65,-70,-75,-80};
int BLx[]= {-80,-75,-70,-65,-60,-55,-50,-45,-40,-35,-30,-25,-20,-15,-10, -5,  0,  5, 10, 15, 20, 25, 30, 35, 40, 45, 50, 55, 60, 65, 70, 75, 80,     80, 75, 70, 65, 60, 55, 50, 45, 40, 35, 30, 25, 20, 15, 10,  5,  0, -5,-10,-15,-20,-25,-30,-35,-40,-45,-50,-55,-60,-65,-70,-75,-80};
 
int FRy[]= {  0, -5,-10,-10,-10,-11,-11,-11,-12,-12,-12,-13,-13,-13,-14,-14,-14,-15,-15,-15,-16,-16,-16,-17,-17,-17,-18,-18,-18,-19,-15,-10,  0,      0,  1,  2,  3,  4,  5,  6,  7,  8, 10, 12, 14, 16, 18, 21, 25, 30, 25, 21, 18, 16, 14, 12, 10,  8,  7,  6,  5,  4,  3,  2,  1,  0};
int FLy[]= {  0,  1,  2,  3,  4,  5,  6,  7,  8, 10, 12, 14, 16, 18, 21, 25, 30, 25, 21, 18, 16, 14, 12, 10,  8,  7,  6,  5,  4,  3,  2,  1,  0,      0, -5,-10,-10,-10,-11,-11,-11,-12,-12,-12,-13,-13,-13,-14,-14,-14,-15,-15,-15,-16,-16,-16,-17,-17,-17,-18,-18,-18,-19,-15,-10,  0};
int BRy[]= {  0,  1,  2,  3,  4,  5,  6,  7,  8, 10, 12, 14, 16, 18, 21, 25, 30, 25, 21, 18, 16, 14, 12, 10,  8,  7,  6,  5,  4,  3,  2,  1,  0,      0, -5,-10,-10,-10,-11,-11,-11,-12,-12,-12,-13,-13,-13,-14,-14,-14,-15,-15,-15,-16,-16,-16,-17,-17,-17,-18,-18,-18,-19,-15,-10,  0};
int BLy[]= {  0, -5,-10,-10,-10,-11,-11,-11,-12,-12,-12,-13,-13,-13,-14,-14,-14,-15,-15,-15,-16,-16,-16,-17,-17,-17,-18,-18,-18,-19,-15,-10,  0,      0,  1,  2,  3,  4,  5,  6,  7,  8, 10, 12, 14, 16, 18, 21, 25, 30, 25, 21, 18, 16, 14, 12, 10,  8,  7,  6,  5,  4,  3,  2,  1,  0};
 
int lgTab = sizeof(FRx)/sizeof(int);
 
void setup() {
  pinMode(0,INPUT_PULLUP);                // start/reset button attachment
  Serial.begin(9600);
  for(int i=2;i<=9;i++) pinMode(i,OUTPUT);
 
  // Servos angle initialization
  int a=20;
  if(OnOff[FRLS]) set_position_degrees(FRLS,    a+Err[FRLS]);  // FR Front Right leg - LS Left   Servo
  if(OnOff[FRRS]) set_position_degrees(FRRS,180-a+Err[FRRS]);  // FR Front Right leg - RS Right  Servo
  if(OnOff[FLLS]) set_position_degrees(FLLS,    a+Err[FLLS]);  // FL Front Left  leg - LS Left   Servo
  if(OnOff[FLRS]) set_position_degrees(FLRS,180-a+Err[FLRS]);  // FL Front Left  leg - RS Right  Servo
  if(OnOff[BRLS]) set_position_degrees(BRLS,    a+Err[BRLS]);  // BR Back  Right leg - LS Left   Servo
  if(OnOff[BRRS]) set_position_degrees(BRRS,180-a+Err[BRRS]);  // BR Back  Right leg - RS Right  Servo
  if(OnOff[BLLS]) set_position_degrees(BLLS,    a+Err[BLLS]);  // BL Back  Left  leg - LS Left   Servo
  if(OnOff[BLRS]) set_position_degrees(BLRS,180-a+Err[BLRS]);  // BL Back  Left  leg - RS Right  Servo
 
  Serial.print("\n\t To start, click on the Start button");
  while( digitalRead(0) )
  {
    unsigned long start_time_period=micros();
    envoyer_pulse();
    while(micros()-start_time_period<PERIODE_US);
  } 
  Serial.print("\n\t Started");
}
 
void loop() {
  static uint32_t c;
  unsigned long start_time_period=micros();
  envoyer_pulse();
  calculer_nouvelle_position(c); //nb : tout votre code sera exécuté depuis l'intérieur de cette fonction
  //attendre la fin de la période
  while(micros()-start_time_period<PERIODE_US);
  c++;
}
 
 
void calculer_nouvelle_position(uint32_t index){
  int i=index % lgTab;
  InverseKinematics(FRx[i],FRy[i],FRLS,FRRS);
//  InverseKinematics(BLx[i],BLy[i],BLLS,BLRS);
//  InverseKinematics(FLx[i],FLy[i],FLLS,FLRS);
//  InverseKinematics(BRx[i],BRy[i],BRLS,BRRS);
}
 
void set_position_degrees(byte pin, int angle_deg){
  uint16_t duree_us=map(angle_deg, angle_min[pin], angle_max[pin], duree_pulse_us_min[pin], duree_pulse_us_max[pin]);
  duree_pulse_us[pin]=duree_us;
}
 
void envoyer_pulse(){
  unsigned long start_time=micros();
  //mise à 1 de tous les pins
  PORTD |= B11111100; //mise à 1 des pins 2 à 7 du port D (ie pins 2 à 7 de l'arduino)
  PORTB |= B00000011; //mise à 1 des pins 0à 1 du port B (ie pins 8 et 9 de l'arduino)
  
  while( (PORTD & 0B11111100) | (PORTB & 0B00000011) ){  //tant qu'au moins un des pins est à 1
    unsigned long duree_actuelle_du_pulse=micros()-start_time;  //durée écoulée depuis le début du pulse
    if(duree_actuelle_du_pulse>=duree_pulse_us[2])  PORT_DE_PIN_2 &= ZERO_DU_PIN_2;
    if(duree_actuelle_du_pulse>=duree_pulse_us[3])  PORT_DE_PIN_3 &= ZERO_DU_PIN_3;
    if(duree_actuelle_du_pulse>=duree_pulse_us[4])  PORT_DE_PIN_4 &= ZERO_DU_PIN_4;
    if(duree_actuelle_du_pulse>=duree_pulse_us[5])  PORT_DE_PIN_5 &= ZERO_DU_PIN_5;
    if(duree_actuelle_du_pulse>=duree_pulse_us[6])  PORT_DE_PIN_6 &= ZERO_DU_PIN_6;
    if(duree_actuelle_du_pulse>=duree_pulse_us[7])  PORT_DE_PIN_7 &= ZERO_DU_PIN_7;
    if(duree_actuelle_du_pulse>=duree_pulse_us[8])  PORT_DE_PIN_8 &= ZERO_DU_PIN_8;
    if(duree_actuelle_du_pulse>=duree_pulse_us[9])  PORT_DE_PIN_8 &= ZERO_DU_PIN_9;
  }
}
 
void InverseKinematics(int Px, int Py, int LS, int RS){
  const float A1x=0, A1y=140, A2x=0, A2y=140;                   // Values of servos positions
  const float a1=112, c1=80, a2=112, c2=80;                     // Values of leg sizes lengths
 
  float d=A1y-Py, e=Px;                                   // Calculation of inverse kinematics
  float b=sqrt((d*d)+(e*e));                              // Calculation of inverse kinematics
  float S=acos(d/b);  if(e<0)S=(-S);                      // Calculation of inverse kinematics
  float A12=acos(((b*b)+(c1*c1)-(a1*a1))/(2*b*c1));       // Calculation of inverse kinematics
  float A22=acos(((b*b)+(c2*c2)-(a2*a2))/(2*b*c2));       // Calculation of inverse kinematics
  float A11=(PI/2)-A12+S;                                 // Calculation of inverse kinematics
  float A21=(PI/2)-A22-S;                                 // Calculation of inverse kinematics
  int S1=round(A11*57.296);                               // left servo angle in degree
  int S2=round(180-(A21*57.296));                         // right servo angle in degree
 
// DEBUG
  Serial.print("\n\t Position to reach : Px=");Serial.print(Px);Serial.print("  Py=");Serial.print(Py);
//  Serial.print("\n\t d=");Serial.print(d);Serial.print("\t\t e=");Serial.print(e);
//  Serial.print("\t\t b=");Serial.print(b);Serial.print("\t\t S=");Serial.print(S*57.296);
//  Serial.print("\n\t A11=");Serial.print(A11*57.296);Serial.print("\t\t A12=");Serial.print(A12*57.296);
//  Serial.print("\t\t A22=");Serial.print(A22*57.296);Serial.print("\t\t A21=");Serial.print(A21*57.296);
//  Serial.print("\n\t Result of calculations, angles of the servos");
//  Serial.print("\n\t S1=");Serial.print(S1);Serial.print("°\t\t\t S2=");Serial.print(S2);Serial.print("°");
 
  if ( b>(a1+c1) ){
    Serial.print("\n\t Target point Px=");Serial.print(Px);Serial.print(" Py=");Serial.print(Py);Serial.print("\t b=");Serial.print(b);Serial.print(" > ");
    Serial.print(a1+c1);Serial.print(" is too long. Target impossible to reach   !!!!!");
    return;
  }
  if (S1<0){
    Serial.print("\n\t Position to reach : Px=");Serial.print(Px);Serial.print("  Py=");Serial.print(Py);Serial.print("\t angle S1<0° is not reachable   !!!!!");
    return;
  }
  if (S2>180){
    Serial.print("\n\t Position to reach : Px=");Serial.print(Px);Serial.print("  Py=");Serial.print(Py);Serial.print("\t angle S2<0° is not reachable   !!!!!");
    return;
  }
  if (S1>120){
    Serial.print("\n\t Position to reach : Px=");Serial.print(Px);Serial.print("  Py=");Serial.print(Py);Serial.print("\t angle S1>140° is not reachable   !!!!!");
    return;
  }
  if (S2<60){
    Serial.print("\n\t Position to reach : Px=");Serial.print(Px);Serial.print("  Py=");Serial.print(Py);Serial.print("\t angle S2<40° is not reachable   !!!!!");
    return;
  }
//  Serial.print("\t executed command");
  if (OnOff[LS]) set_position_degrees(LS,S1+Err[LS]);   // set target Left servo position if servo switch is On
  if (OnOff[RS]) set_position_degrees(RS,S2+Err[RS]);   // set target Right servo position if servo switch is On
}

[Oracid]

J'ai testé sur mon Quad.

Malheureusement, c'est très lent. Au moins, 10 fois plus lent, pas un petit peu.

Et je ne sais pas comment régler la vitesse.

[Sandro]

Le problème est simple : sur une période, tu as 66 points. Une commande prends PERIODE_US=20 000µs= 1/50 s. Du coup, pour faire un "pas" (une période), il te faut 66*(1/50)=1.32 secondes. C'est sur, qu'à se rythme là, tu ne vas pas bien vite.

 

Que se passait-il sans ton ancien code : tu envoyais les commandes à intervalles beaucoup plus rapprochés (inférieurs à 20ms). Mais la librairie servo n'envoyait quand même qu'un pulse toutes les 20ms pour chaque moteur : il y a donc des points qui sont ignorés, sans que tu maîtrise lesquels (d'ailleurs, ce n'est même pas forcément le même qui est ignoré pour tous les servos).

 

 

Pour résumer, le nouveau code fait ce que tu lui dis, et passe par tous les points (un tous les 20ms), alors que l'ancien s'autorisait à en sauter un bon nombre sans que tu maîtrise lesquels.

 

 

Donc pour retrouver le comportement de l'ancien code, il suffit de ne garder que un point sur 10 (si tu dis que tu vas 10 fois moins vite) : c'est ce que faisait déjà l'ancien code, sauf que tu n'avais aucune maîtrise sur quel point tu laisse tomber.

 

Un autre piste pour aller plus vite (et augmenter la précision de la trajectoire), c'est de commander les moteurs plus rapidement : pour ça, il suffit de réduire la valeur de PERIODE_US. Pour celà, il faut que tu respectes 2 contraintes:

- les servos doivent fonctionner avec PERIODE_US aussi faible (ceux que j'ai testé, je peux réduire autant que je veux, d'autres semblerait-il ne peuvent pas descendre trop pas)

- il faut qu'il reste possible de respecter une période de PERIODE_US. Si on appelle T_pulse_max la durée du plus long pulse possible (2.5ms dans ton dernier code), T_processing le temps d'exécution de calculer_nouvelle_position et T_marge une petite marge de sécurité (je pense que 0.5ms devrait suffire), alors il faut que T_pulse_max+T_processing+T_marge <= PERIODE_US

 

 

Ce que je te suggèrerais, c'est de combiner les deux :

1) tu cherches la plus petite valeur possible pour PERIODE_US. Cette valeur ne bougera plus ensuite dans tes tests

2) tu adaptes la distance entre les points pour adapter la vitesse

[Oracid]

Mouuuuuuais !

 

Je ne suis pas convaincu. Pour  PERIODE_US, j'y avais bien pensé, d'autant que c'était documenté dans les commentaires. J'attendais ta confirmation.

Pour le reste, cela voudrait dire qu'il suffirait de faire moins de pas dans un mouvement pour aller plus vite. Ce n'est pas le cas !

C'est, évidemment, ce que je pensais intuitivement, mais les tests montrent le contraire.

 

De plus, d'expérience, je fais bien la différence entre un mouvement avec un pas long et celui avec un pas court.

Que le servo n'arrive pas au bout de sa consigne, d'accord, mais qu'il la saute, non, je ne pense pas.

 

Je vais faire encore quelques tests, après je passe à autre chose. . . 

[pat92fr]

Je comprends ce que Sandro explique.

 

La carte Arduino envoie une consigne toutes les 20ms (c'est ce que fait la bibliothèque Servo par défaut). Ton code tente d'en envoyer plus de 50 consignes par seconde en appelant la fonction Servo, sans respecter le temps de pause de 20ms. Certaines consignes sont perdues car elles ne sortent même pas de la carte Arduino. En fait, le servo ne saute rien du tout, en fait, il ne reçoit qu'une partie des commande que tu cherches à envoyer. Courage !

 

Le code de Sandro résout ce problème. Tu peux en envoyer toutes les 2.5ms avec la bonne configuration.

 

Patrick.

[Sandro]

Pour le reste, cela voudrait dire qu'il suffirait de faire moins de pas dans un mouvement pour aller plus vite. Ce n'est pas le cas !

C'est, évidemment, ce que je pensais intuitivement, mais les tests montrent le contraire.

Ce n'était probablement pas le cas sur ton ancien code car tu envoyais les pas "trop vite", l'arduino n'ayant même pas le temps de tous les transmettre aux servos.

Avec mon nouveau code, le timming est régulier : SI la vitesse des servos est inférieure à leur vitesse max ALORS le temps de parcours est égal à NBR_PAS*PERIODE_US. Donc pour accélérer ta trajectoire, tu peux "tout simplement" réduire le nombre de pas sur la trajectoire. Attention : si les servos n'arrivent plus à suivre la vitesse demandée, alors ce raisonnement n'est plus valable (et on peut même avoir l'effet inverse, en n'allant plus au bout des enjambées)

 

 

Que le servo n'arrive pas au bout de sa consigne, d'accord, mais qu'il la saute, non, je ne pense pas.

Comme l'explique Patrick, le problème est que l'arduino (avec la librairie servo) n’envoie une consigne (ie un pulse) au servo que toutes les 20ms. Si pendant cette durée tu as changé 4 fois la valeur de la consigne, alors seul la dernière valeur sera effectivement transmise au servo.

 

 

 

Le code de Sandro résout ce problème. Tu peux en envoyer toutes les 2.5ms avec la bonne configuration.

Pas tout à fait : il faut ajouter le temps de calculer les nouvelles consignes. Du coup, je dirais plutôt une consigne toutes les 5ms, à moins de pré-calculer la longueur des pulses par servo, plutôt que de les calculer en cours d'exécution à partir des trajectoires en x et y. Si on pré-calcule tout, je pense qu'il doit être possible d'envoyer des données toutes les 3ms
 

 

[Oracid]

Je comprends parfaitement ce que vous dites, mais il n'en reste pas moins qu'au final, avec des pas de 5mm, je vais plus vite qu'avec des pas de 10mm, pour une amplitude équivalente.

C'est donc en contradiction avec ce que vous dites, à moins que je me sois trompé dans mes tests. C'est parfaitement possible.

Je vais donc tester cela avec mon programme tel qu'il est, pour éventuellement vous en convaincre, puis je testerai avec le programme de Sandro.

 

Bon, là, avec les fêtes, c'est un peu compliqué, mais je vais faire de mon mieux.

[Sandro]

Avec ton programme, c'est difficile de prédire comment ça vas se comporter, vu qu'il agit de manière "imprévisible". Du coup, il est possible qu'avec des pas plus petits il soit plus rapide.

 

Si tu te bases sur mon code, alors, tant que les moteurs suivront, je suis convaincu que plus les pas sont longs, plus le robot ira vite (nb : j'insiste : tant que les moteurs suivent : si les moteurs ne tiennent plus la vitesse de déplacement, alors je ne sais pas comment ça vas se comporter, mais il est possible que la situation empire)

[Oracid]

Ok, je vais commencer avec ton programme.

[Oracid]

Bravo Sandro !

 

J'ai baissé la variable PERIODE_US de 20000us à 8000us pour avoir une vitesse acceptable avec un pas de 5mm. Pas génial, environ 250cm en 5 secondes.

Puis, je suis passé au pas de 10mm. Là, tu as égalé mon record à 510cm en 5 secondes, du premier coup.

Evidemment, l'inconvénient, c'est qu'avec un pas de 10mm, ça sautille beaucoup plus qu'avec un pas de 5mm. Les mouvements sont plus violents.

Du coup, les tests sont plus difficiles à faire, et je cours après la bête pour la remettre dans le droit chemin. Malheureusement, il pleut, et je ne peux pas utiliser le rail en extérieur.

 

Voici les images Excel des 2 pas et du mouvement que j'ai utilisé.

 

 

J'ai mis les tableaux duree_pulse_us_min[10] et duree_pulse_us_max[10] respectivement à 544 et 2400 comme indiqué dans cette page https://www.arduino....s/servo/attach/

 

Voici le code.

Spoiler

// Version Sandro V3 - 19/12/2020
// Remarques : cela fonctionne très bien.
 
void(* resetFunc) (void) = 0;           // soft reset
 
//équivalences pin/port
//  2   PD2
//  3   PD3
//  4   PD4
//  5   PD5
//  6   PD6
//  7   PD7
//  8   PB0
//  9   PB1
 
//octets avec un 0 à l'endroit correspondant à un pin dans les registres et 1 ailleurs
#define ZERO_DU_PIN_2 B11111011
#define ZERO_DU_PIN_3 B11110111
#define ZERO_DU_PIN_4 B11101111
#define ZERO_DU_PIN_5 B11011111
#define ZERO_DU_PIN_6 B10111111
#define ZERO_DU_PIN_7 B01111111
#define ZERO_DU_PIN_8 B11111110
#define ZERO_DU_PIN_9 B11111101
 
//nom du pot correspondant à chaque pin
#define PORT_DE_PIN_2 PORTD
#define PORT_DE_PIN_3 PORTD
#define PORT_DE_PIN_4 PORTD
#define PORT_DE_PIN_5 PORTD
#define PORT_DE_PIN_6 PORTD
#define PORT_DE_PIN_7 PORTD
#define PORT_DE_PIN_8 PORTB
#define PORT_DE_PIN_9 PORTB
 
//#define PERIODE_US 20000  // PÉRIODE DU PWM, EN MICROSECONDES : valeur "par défaut" de 20000µs=20ms : sur la plupart des servos cette durée peut être réduite, pour certains qu'un peu, pour d'autres quasiment jusqu'à la longueur de l'impulsion la plus longue. NB : cette durée correspon également à la période d'asservissement (ie à quelle période on vas obtenir une nouvelle position pour le servo)
#define PERIODE_US 8000  // PÉRIODE DU PWM, EN MICROSECONDES : valeur "par défaut" de 20000µs=20ms : sur la plupart des servos cette durée peut être réduite, pour certains qu'un peu, pour d'autres quasiment jusqu'à la longueur de l'impulsion la plus longue. NB : cette durée correspon également à la période d'asservissement (ie à quelle période on vas obtenir une nouvelle position pour le servo)
 
uint16_t duree_pulse_us[10]={0,0,1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500};   //POSITION INITIALE, en durée de pulse en µs. NB : cette position n'a d'influance que si on appelle envoyer_pulse un certain nombre de fois sans avoir auparavent spécifié de position avec set_position_degrees
uint16_t duree_pulse_us_min[10]={0,0, 544,  544,  544,  544,  544,  544,  544,  544}; //DURÉE MINIMALE D'UN PULSE : à adapter selon le moteur choisit (1000 pour certains, un peu moins pour d'autres, peut potentiellement varier d'un servo à l'autre). NB : cette valeur peut être supérieure min atteignable, dans ce cas le moteur n'irra simplement pas jusqu'au bout de sa course
uint16_t duree_pulse_us_max[10]={0,0,2400, 2400, 2400, 2400, 2400, 2400, 2400, 2400}; //DURÉE MAXIMALE D'UN PULSE : idem
int angle_min[10]={0,0,0, 0, 0, 0, 0, 0, 0, 0};   //ANGLE CORRESPONDANT À LA DURÉE_PULSE_US_MIN
int angle_max[10]={0,0,180, 180, 180, 180, 180, 180, 180, 180};   //ANGLE CORRESPONDANT À LA DURÉE_PULSE_US_MIN
 
unsigned long start;
const int nb=10;                         // Number of servos
int Srv[nb];                          // Servos table 
int OnOff[nb]={0,0,1,1,1,1,1,1,1,1};        // Servos table on/off
int FRLS=2, FRRS=3, FLLS=4, FLRS=5, BRLS=6, BRRS=7, BLLS=8, BLRS=9;
//***************** Correction for 50° LS and RS servos position     **************************************
// servos         FRLS  FRRS  FLLS  FLRS  BRLS  BRRS  BLLS  BLRS modify these values according to your servos
int Err[nb]={0,0,   0,    1,    5,    4,    5,    5,     11,   3    };  
//*********************************************************************************************************
 
// 21/12/2020 - symetric Soucoupe volante V13-71 - PERIODE_US 8000, pas=10mm, Ay=165, a1=120, c1=88, 510cm/8V
int FRx[]= { 80, 70, 60, 50, 40, 30, 20, 10,  0,-10,-20,-30,-40,-50,-60,-70,-80,    -80,-70,-60,-50,-40,-30,-20,-10,  0, 10, 20, 30, 40, 50, 60, 70, 80};
int FLx[]= { 80, 70, 60, 50, 40, 30, 20, 10,  0,-10,-20,-30,-40,-50,-60,-70,-80,    -80,-70,-60,-50,-40,-30,-20,-10,  0, 10, 20, 30, 40, 50, 60, 70, 80};
int BRx[]= {-80,-70,-60,-50,-40,-30,-20,-10,  0, 10, 20, 30, 40, 50, 60, 70, 80,     80, 70, 60, 50, 40, 30, 20, 10,  0,-10,-20,-30,-40,-50,-60,-70,-80};
int BLx[]= {-80,-70,-60,-50,-40,-30,-20,-10,  0, 10, 20, 30, 40, 50, 60, 70, 80,     80, 70, 60, 50, 40, 30, 20, 10,  0,-10,-20,-30,-40,-50,-60,-70,-80};
 
int FRy[]= {  0,-10,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-10,  0,      0,  8, 12, 16, 20, 22, 24, 25, 25, 25, 24, 22, 20, 16, 12,  8,  0};
int FLy[]= {  0,  8, 12, 16, 20, 22, 24, 25, 25, 25, 24, 22, 20, 16, 12,  8,  0,      0,-10,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-10,  0};
int BRy[]= {  0,  8, 12, 16, 20, 22, 24, 25, 25, 25, 24, 22, 20, 16, 12,  8,  0,      0,-10,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-10,  0};
int BLy[]= {  0,-10,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-10,  0,      0,  8, 12, 16, 20, 22, 24, 25, 25, 25, 24, 22, 20, 16, 12,  8,  0};
 
int lgTab = sizeof(FRx)/sizeof(int);
 
void setup() {
  pinMode(0,INPUT_PULLUP);                // start/reset button attachment
  Serial.begin(9600);
  for(int i=2;i<=9;i++) pinMode(i,OUTPUT);
 
  // Servos angle initialization
  int a=50;
  if(OnOff[FRLS]) set_position_degrees(FRLS,    a+Err[FRLS]);  // FR Front Right leg - LS Left   Servo
  if(OnOff[FRRS]) set_position_degrees(FRRS,180-a+Err[FRRS]);  // FR Front Right leg - RS Right  Servo
  if(OnOff[FLLS]) set_position_degrees(FLLS,    a+Err[FLLS]);  // FL Front Left  leg - LS Left   Servo
  if(OnOff[FLRS]) set_position_degrees(FLRS,180-a+Err[FLRS]);  // FL Front Left  leg - RS Right  Servo
  if(OnOff[BRLS]) set_position_degrees(BRLS,    a+Err[BRLS]);  // BR Back  Right leg - LS Left   Servo
  if(OnOff[BRRS]) set_position_degrees(BRRS,180-a+Err[BRRS]);  // BR Back  Right leg - RS Right  Servo
  if(OnOff[BLLS]) set_position_degrees(BLLS,    a+Err[BLLS]);  // BL Back  Left  leg - LS Left   Servo
  if(OnOff[BLRS]) set_position_degrees(BLRS,180-a+Err[BLRS]);  // BL Back  Left  leg - RS Right  Servo
 
  Serial.print("\n\t To start, click on the Start button");
  while( digitalRead(0) )
  {
    unsigned long start_time_period=micros();
    envoyer_pulse();
    while(micros()-start_time_period<PERIODE_US);
  } 
  Serial.print("\n\t Started");
  start=millis();
}
 
void loop() {
  static uint32_t c;
  unsigned long start_time_period=micros();
  envoyer_pulse();
  calculer_nouvelle_position(c); //nb : tout votre code sera exécuté depuis l'intérieur de cette fonction
  //attendre la fin de la période
  while(micros()-start_time_period<PERIODE_US);
  c++;
 
  if( millis()-start > 5000)  resetFunc();         // 5 secondes running
 
}
 
 
void calculer_nouvelle_position(uint32_t index){
  int i=index % lgTab;
  InverseKinematics(FRx[i],FRy[i],FRLS,FRRS);
  InverseKinematics(BLx[i],BLy[i],BLLS,BLRS);
  InverseKinematics(FLx[i],FLy[i],FLLS,FLRS);
  InverseKinematics(BRx[i],BRy[i],BRLS,BRRS);
}
 
void set_position_degrees(byte pin, int angle_deg){
  uint16_t duree_us=map(angle_deg, angle_min[pin], angle_max[pin], duree_pulse_us_min[pin], duree_pulse_us_max[pin]);
  duree_pulse_us[pin]=duree_us;
}
 
void envoyer_pulse(){
  unsigned long start_time=micros();
  //mise à 1 de tous les pins
  PORTD |= B11111100; //mise à 1 des pins 2 à 7 du port D (ie pins 2 à 7 de l'arduino)
  PORTB |= B00000011; //mise à 1 des pins 0à 1 du port B (ie pins 8 et 9 de l'arduino)
  
  while( (PORTD & 0B11111100) | (PORTB & 0B00000011) ){  //tant qu'au moins un des pins est à 1
    unsigned long duree_actuelle_du_pulse=micros()-start_time;  //durée écoulée depuis le début du pulse
    if(duree_actuelle_du_pulse>=duree_pulse_us[2])  PORT_DE_PIN_2 &= ZERO_DU_PIN_2;
    if(duree_actuelle_du_pulse>=duree_pulse_us[3])  PORT_DE_PIN_3 &= ZERO_DU_PIN_3;
    if(duree_actuelle_du_pulse>=duree_pulse_us[4])  PORT_DE_PIN_4 &= ZERO_DU_PIN_4;
    if(duree_actuelle_du_pulse>=duree_pulse_us[5])  PORT_DE_PIN_5 &= ZERO_DU_PIN_5;
    if(duree_actuelle_du_pulse>=duree_pulse_us[6])  PORT_DE_PIN_6 &= ZERO_DU_PIN_6;
    if(duree_actuelle_du_pulse>=duree_pulse_us[7])  PORT_DE_PIN_7 &= ZERO_DU_PIN_7;
    if(duree_actuelle_du_pulse>=duree_pulse_us[8])  PORT_DE_PIN_8 &= ZERO_DU_PIN_8;
    if(duree_actuelle_du_pulse>=duree_pulse_us[9])  PORT_DE_PIN_8 &= ZERO_DU_PIN_9;
  }
}
 
void InverseKinematics(int Px, int Py, int LS, int RS){
  const float A1x=0, A1y=165, A2x=0, A2y=165;                   // Values of servos positions
  const float a1=120, c1=80, a2=120, c2=80;                     // Values of leg sizes lengths
 
  float d=A1y-Py, e=Px;                                   // Calculation of inverse kinematics
  float b=sqrt((d*d)+(e*e));                              // Calculation of inverse kinematics
  float S=acos(d/b);  if(e<0)S=(-S);                      // Calculation of inverse kinematics
  float A12=acos(((b*b)+(c1*c1)-(a1*a1))/(2*b*c1));       // Calculation of inverse kinematics
  float A22=acos(((b*b)+(c2*c2)-(a2*a2))/(2*b*c2));       // Calculation of inverse kinematics
  float A11=(PI/2)-A12+S;                                 // Calculation of inverse kinematics
  float A21=(PI/2)-A22-S;                                 // Calculation of inverse kinematics
  int S1=round(A11*57.296);                               // left servo angle in degree
  int S2=round(180-(A21*57.296));                         // right servo angle in degree
 
// DEBUG
//  Serial.print("\n\t Position to reach : Px=");Serial.print(Px);Serial.print("  Py=");Serial.print(Py);
//  Serial.print("\n\t d=");Serial.print(d);Serial.print("\t\t e=");Serial.print(e);
//  Serial.print("\t\t b=");Serial.print(b);Serial.print("\t\t S=");Serial.print(S*57.296);
//  Serial.print("\n\t A11=");Serial.print(A11*57.296);Serial.print("\t\t A12=");Serial.print(A12*57.296);
//  Serial.print("\t\t A22=");Serial.print(A22*57.296);Serial.print("\t\t A21=");Serial.print(A21*57.296);
//  Serial.print("\n\t Result of calculations, angles of the servos");
//  Serial.print("\n\t S1=");Serial.print(S1);Serial.print("°\t\t\t S2=");Serial.print(S2);Serial.print("°");
 
  if ( b>(a1+c1) ){
    Serial.print("\n\t Target point Px=");Serial.print(Px);Serial.print(" Py=");Serial.print(Py);Serial.print("\t b=");Serial.print(b);Serial.print(" > ");
    Serial.print(a1+c1);Serial.print(" is too long. Target impossible to reach   !!!!!");
    return;
  }
  if (S1<0){
    Serial.print("\n\t Position to reach : Px=");Serial.print(Px);Serial.print("  Py=");Serial.print(Py);Serial.print("\t angle S1<0° is not reachable   !!!!!");
    return;
  }
  if (S2>180){
    Serial.print("\n\t Position to reach : Px=");Serial.print(Px);Serial.print("  Py=");Serial.print(Py);Serial.print("\t angle S2<0° is not reachable   !!!!!");
    return;
  }
  if (S1>120){
    Serial.print("\n\t Position to reach : Px=");Serial.print(Px);Serial.print("  Py=");Serial.print(Py);Serial.print("\t angle S1>140° is not reachable   !!!!!");
    return;
  }
  if (S2<60){
    Serial.print("\n\t Position to reach : Px=");Serial.print(Px);Serial.print("  Py=");Serial.print(Py);Serial.print("\t angle S2<40° is not reachable   !!!!!");
    return;
  }
//  Serial.print("\t executed command");
  if (OnOff[LS]) set_position_degrees(LS,S1+Err[LS]);   // set target Left servo position if servo switch is On
  if (OnOff[RS]) set_position_degrees(RS,S2+Err[RS]);   // set target Right servo position if servo switch is On
}

[Sandro]

Bonjour,

le fait que ça sautille plus, ça peut venir du fait que le pas est 10mm, mais aussi simplement du fait que ça ailles plus vite (petit test si tu veux vérifier : garde le pas à 10mm, mais mets PERIODE_US à 16000 µs : tu devrais aller à la même vitesse qu'avec le pas de 5mm à 8000µs, et tu pourra voir si ça sautille plus avec le pas de 10mm qu'avec celui de 5mm à vitesse égale).

 

Sinon, tu peux aussi tenter de réduire PERIODE_US à 4000 µs (si ton calcul est assez rapide pour tenir la en 1500µs) et reprendre un pas de 5mm. Ou réduire à 0.66*8000 µs, et prendre un pas de 6.66mm