Bonjour,
Je programme en C Arduino pour asservir les vitesses d'un moteur Dynamixel AX-18A. Pour cela, on dispose de registres d'écriture de la vitesse cible (consigne) et de registres de lecture de la vitesse courante (mesure).
Lorsque j'indique des consignes de vitesses au moteur, celui-ci ne bouge pas.
Lorsque j'indique des consignes de postions et de vitesses au moteur, celui-ci bouge mais ne respecte pas vraiment les consignes de vitesse (fichier joint vitessesKO ; erreur quadratique moyenne : 1.1624). Pourtant, la fréquence des mesures n'est pas très élevée, puisqu'il y a une mesure tous les dt=50 ms.
Par contre, lorsque je fais des "fausses" mesures de vitesses, c'est-à-dire lorsque je dérive les positions mesurées, l'erreur quadratique moyenne est beaucoup plus faible : 0.0028786 (fichier joint vitessesOK).
Est-ce que quelqu'un saurait imposer des vitesses sur un moteur Dynamixel ?
Merci d'avance ; et je vous joins le code C qui tente d'imposer des positions et des vitesses (code dont le résultat est représenté sur le fichier joint vitessesKO) :
#include <Commander.h>
#include <ax12.h> //Pour les commandes vers Dynamixel
#include <BioloidController.h>
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
int i=0;
int positionMesuree;
int vitesseMesuree;
double positionMesureeRad;
double vitesseMesureeRad;
int tempsDepart, tempsCourant;
double positionSim[46]={0,0.00044979,0.0017992,0.0040481,0.0071966,0.011245,0.016192,0.02204,0.028787,0.036433,0.044979,0.054425,0.06477,0.076014,0.088159,0.1012,0.11515,0.12999,0.14573,0.16237,0.17992,0.19836,0.2177,0.23786,0.25756,0.27637,0.29428,0.31128,0.32739,0.3426,0.35691,0.37031,0.38282,0.39443,0.40514,0.41495,0.42386,0.43187,0.43898,0.44519,0.45051,0.45492,0.45843,0.46105,0.46276,0.46357};
double vitesseSim[46]={0,0.089958,0.17992,0.26987,0.35983,0.44979,0.53975,0.62971,0.71966,0.80962,0.89958,0.98954,1.0795,1.1695,1.2594,1.3494,1.4393,1.5293,1.6192,1.7092,1.7992,1.8891,1.9791,2.0155,1.9256,1.8356,1.7456,1.6557,1.5657,1.4758,1.3858,1.2959,1.2059,1.1159,1.026,0.93602,0.84606,0.7561,0.66615,0.57619,0.48623,0.39627,0.30631,0.21636,0.1264,0.03644};
double positionDynamixDeci; //Nombre decimal entre 0 et 1023
//De 0 a 1023 : sens trigo
//De 1024 a 2047 : sens anti-trigo
double vitesseDynamixDeci;
int vitesseDynamixEntier;
int idMoteur=1;
int attente=24;
//Fonction pour convertir radians de la sim avec 0 de Dynamixel, en position Dynamixel
int radVersDynamix(double positionSim)
{
double positionDynamixDeci;
int positionDynamixEntier;
positionDynamixDeci = positionSim * 195.378; //Depasse 1024 ?
positionDynamixEntier = (int) positionDynamixDeci;
return positionDynamixEntier;
}
//Fonction pour convertir position mesuree Dynamixel en radians
double dynamixVersRad(int positionMesureeDynamix)
{
double positionMesureeRad;
double positionMesureeDynamixDeci = (double) positionMesureeDynamix;
positionMesureeRad = positionMesureeDynamixDeci * 5.118 * 0.001;
return positionMesureeRad;
}
//Fonction pour convertir vitesses angulaires de la sim en vitesse Dynamixel
int vitesseRadVersDynamix(double vitesseSim)
{
double vitesseDynamixDeci;
int vitesseDynamixEntier;
vitesseDynamixDeci = vitesseSim * 195.378;
if(vitesseSim>=0)
{
vitesseDynamixEntier = (int) vitesseDynamixDeci;
}
else
{
vitesseDynamixEntier = 1024 + (int) vitesseDynamixDeci;
}
}
//Fonction pour convertir vitesse mesuree Dynamixel en rad/s
double vitesseDynamixVersRad(int vitesseMesureeDynamix)
{
double vitesseMesureeRad;
//Gestion du sens de deplacement
if(vitesseMesureeDynamix>1023)
{
vitesseMesureeDynamix = 1024 - vitesseMesureeDynamix;
}
double vitesseMesureeDynamixDeci = (double) vitesseMesureeDynamix;
vitesseMesureeRad = vitesseMesureeDynamixDeci * 5.118 * 0.001;
return vitesseMesureeRad;
}
//------------------------------------------------------------------------------
void setup()
{
Serial.begin(115200);
tempsDepart=millis();
SetPosition(idMoteur,0);
delay(2000);
}
void loop()
{
//Mesure des positions
positionMesuree = ax12GetRegister(idMoteur, AX_PRESENT_POSITION_L, 2);
positionMesureeRad = dynamixVersRad(positionMesuree);
//Vitesse cible
ax12SetRegister2(idMoteur, AX_GOAL_SPEED_L, vitesseRadVersDynamix(vitesseSim[i]));
delay(attente);
if(i==0)
{
delay(2000);
}
//Mesure des vitesses
vitesseMesuree = ax12GetRegister(idMoteur, AX_PRESENT_SPEED_L, 2);
vitesseMesureeRad = vitesseDynamixVersRad(vitesseMesuree);
//Horodatage
tempsCourant=millis()-tempsDepart;
//Affichage clair
Serial.print("Iteration #"); Serial.print(i); Serial.print(" : "); Serial.print("Au temps t=");
Serial.print(tempsCourant);
Serial.print(" Mesure position en rad="); Serial.print(positionMesureeRad);
Serial.print(" / Consigne de vitesse en rad/s="); Serial.print(vitesseSim[i]);
Serial.print(" / Mesure vitesse en rad/s="); Serial.println(vitesseMesureeRad);
//Affichage pour Matlab
/*
Serial.print(tempsCourant); Serial.print(","); Serial.print(positionSim[i]); Serial.print(",");
Serial.print(positionMesureeRad); Serial.print(",");
Serial.print(vitesseSim[i]); Serial.print(",");
Serial.print(vitesseMesureeRad); Serial.println(";");
*/
i++;
}
Fichier(s) joint(s)
-
vitessesKO.png 14,57 Ko
0 téléchargement(s)
-
vitessesOK.png 28,73 Ko
0 téléchargement(s)
