Keuronde/Feetech_PlateformIO
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Commande en vitesse - paramètre du PI en vitesse aux valeur par defaut.

Browse Source
This commit is contained in:
Samuel 2025-08-12 14:49:11 +02:00
parent 18fa86c539
commit 3103c237df
1 changed files with 46 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

68
src/main.cpp
View File

@ -22,10 +22,9 @@ void lire_tous_les_registres(int servo_id);
void setup() { void setup() {
// initialize the digital pin as an output. // initialize the digital pin as an output.
pinMode(LED_BUILTIN, OUTPUT); pinMode(LED_BUILTIN, OUTPUT);
Serial.begin(115200);//sts舵机波特率1000000 Serial.begin(115200); // PC en USB
Serial1.begin(1000000);//sts舵机波特率1000000 Serial1.begin(1000000); // Servo Feetech
sms_sts.pSerial = &Serial1; sms_sts.pSerial = &Serial1;
delay(5000);
} }
@ -98,45 +97,70 @@ enum etat_t test_ping()
} }
enum etat_t configure_servomoteur(){ enum etat_t configure_servomoteur(){
sms_sts.ServoMode(SERVO_ID); sms_sts.writeByte(SERVO_ID, SMS_STS_MODE, 1); // Choix du mode
// On remet par défaut les butées
sms_sts.writeByte(SERVO_ID, 0x09, 0); // Butée min à 0 sms_sts.writeByte(SERVO_ID, 0x09, 0); // Butée min à 0
sms_sts.writeByte(SERVO_ID, 0x0A, 0); // Butée min à 0 sms_sts.writeByte(SERVO_ID, 0x0A, 0); // Butée min à 0
sms_sts.writeByte(SERVO_ID, 0x0B, 0); // Buté max à 0, pour activer le multitour sms_sts.writeByte(SERVO_ID, 0x0B, 4095 & 0xFF ); // Buté max à 0, pour activer le multitour
sms_sts.writeByte(SERVO_ID, 0x0C, 0); // Buté max à 0, pour activer le multitour sms_sts.writeByte(SERVO_ID, 0x0C, (4095>>8) & 0xFF); // Buté max à 0, pour activer le multitour
sms_sts.writeByte(SERVO_ID, 0x1E, 3); // Et les essais sur les basses résolutions
sms_sts.writeByte(SERVO_ID, 0x12, 0x7C); // Registre de "Phase", valeur par défaut 108 (0x6C) sms_sts.writeByte(SERVO_ID, 0x1E, 1);
sms_sts.writeByte(SERVO_ID, 0x12, 0x6C); // Registre de "Phase", valeur par défaut 108 (0x6C)
sms_sts.writeByte(SERVO_ID, 0x25, 10); // Coef P du correcteur en vitesse
sms_sts.writeByte(SERVO_ID, 0x27, 200); // Coef I du correcteur en vitesse
return TERMINE; return TERMINE;
} }
enum etat_t mouvement_servomoteur(){ enum etat_t mouvement_servomoteur(){
static int position=0; static int vitesse=500;
static int temps_pas_ms = 0; static int temps_pas_ms = 0;
static int temps_aff_ms = 0; static int temps_aff_ms = 0;
static int pas_servo = 512; int vitesse_lue;
int position_lue; int commande_moteur;
char tampon[200]; char tampon[200];
/// Toutes les 500 ms /// Toutes les 500 ms
if(millis() - temps_pas_ms > 500 ){ if(millis() - temps_pas_ms > 1000 ){
temps_pas_ms = millis(); temps_pas_ms = millis();
// On avance ou recule de d'un pas // On avance ou recule de d'un pas
position += pas_servo; vitesse = -vitesse;
// si position > 5000 ou position < 0
if(position > 20100 || position < 0){ //sms_sts.writeByte(SERVO_ID, position, 4800);
pas_servo = -pas_servo; sms_sts.WriteSpe(SERVO_ID, vitesse);
}
sms_sts.WritePosEx(SERVO_ID, position, 4800);
} }
if(millis() - temps_aff_ms > 10 ){ if(millis() - temps_aff_ms > 10 ){
temps_aff_ms = millis(); temps_aff_ms = millis();
sprintf(tampon, ">pos_consigne:%d\n", position);
commande_moteur = -1;
commande_moteur = sms_sts.readWord(SERVO_ID, SMS_STS_PRESENT_LOAD_L);
if(commande_moteur != -1){
sprintf(tampon, ">cde_moteur_brute:%d\n", commande_moteur);
Serial.print(tampon);
if(commande_moteur&(1<<10)){
commande_moteur &= 0x3FF;
commande_moteur = - commande_moteur;
}
if(commande_moteur != 32769){
sprintf(tampon, ">cde_moteur:%d\n", commande_moteur);
}
Serial.print(tampon);
}
sprintf(tampon, ">cde_moteur_lib:%d\n", sms_sts.ReadLoad(SERVO_ID));
Serial.print(tampon); Serial.print(tampon);
position_lue = sms_sts.ReadPos(SERVO_ID);
if(position_lue != 32769){ sprintf(tampon, ">vit_consigne:%d\n", vitesse);
sprintf(tampon, ">pos_actuelle:%d\n", position_lue); Serial.print(tampon);
vitesse_lue = sms_sts.ReadSpeed(SERVO_ID);
if(vitesse_lue != 32769){
sprintf(tampon, ">vit_actuelle:%d\n", vitesse_lue);
} }
Serial.print(tampon); Serial.print(tampon);
} }