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

Mode2 semi-fonctionnel

Browse Source
This commit is contained in:
Samuel 2025-08-12 20:00:21 +02:00
parent 3103c237df
commit 3be628497b
1 changed files with 31 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

58
src/main.cpp
View File

@ -97,7 +97,7 @@ enum etat_t test_ping()
}
enum etat_t configure_servomoteur(){
sms_sts.writeByte(SERVO_ID, SMS_STS_MODE, 1); // Choix du mode
sms_sts.writeByte(SERVO_ID, SMS_STS_MODE, 2); // Choix du mode
// On remet par défaut les butées
sms_sts.writeByte(SERVO_ID, 0x09, 0); // Butée min à 0
@ -117,52 +117,56 @@ enum etat_t configure_servomoteur(){
}
enum etat_t mouvement_servomoteur(){
static int vitesse=500;
static int commande_moteur=800;
static int temps_pas_ms = 0;
static int temps_aff_ms = 0;
int vitesse_lue;
int commande_moteur;
int commande_moteur_lue, neg;
char tampon[200];
/// Toutes les 500 ms
if(millis() - temps_pas_ms > 1000 ){
int commande_moteur_tmp;
temps_pas_ms = millis();
// On avance ou recule de d'un pas
vitesse = -vitesse;
commande_moteur = -commande_moteur;
//sms_sts.writeByte(SERVO_ID, position, 4800);
sms_sts.WriteSpe(SERVO_ID, vitesse);
//sms_sts.writeByte(SERVO_ID, 0x2C, 0xFF);
//sms_sts.writeByte(SERVO_ID, 0x2D, 0x05);
commande_moteur_tmp = commande_moteur;
neg = 0;
if(commande_moteur_tmp < 0){
neg = 1;
commande_moteur_tmp = -commande_moteur_tmp;
}
//sms_sts.writeByte(SERVO_ID, 0x2C, commande_moteur_tmp & 0xFF);
//sms_sts.writeByte(SERVO_ID, 0x2D, ((commande_moteur_tmp>>8) & 0xFF) );
sms_sts.writeWord(SERVO_ID, 0x2C, commande_moteur_tmp | neg << 10);
}
if(millis() - temps_aff_ms > 10 ){
temps_aff_ms = millis();
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);
sprintf(tampon, ">cde_moteur:%d\n", commande_moteur);
Serial.print(tampon);
commande_moteur_lue = -1;
commande_moteur_lue = sms_sts.readWord(SERVO_ID, SMS_STS_PRESENT_LOAD_L);
if(commande_moteur_lue != -1){
Serial.print(tampon);
if(commande_moteur&(1<<10)){
commande_moteur &= 0x3FF;
commande_moteur = - commande_moteur;
if(commande_moteur_lue&(1<<10)){
commande_moteur_lue &= 0x3FF;
commande_moteur_lue = - commande_moteur_lue;
}
if(commande_moteur != 32769){
sprintf(tampon, ">cde_moteur:%d\n", commande_moteur);
if(commande_moteur_lue != 32769){
sprintf(tampon, ">cde_moteur_lue:%d\n", commande_moteur_lue);
}
Serial.print(tampon);
}
sprintf(tampon, ">cde_moteur_lib:%d\n", sms_sts.ReadLoad(SERVO_ID));
Serial.print(tampon);
sprintf(tampon, ">vit_consigne:%d\n", vitesse);
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);
}
return EN_COURS;