160 lines
3.7 KiB
C++
160 lines
3.7 KiB
C++
#include <Arduino.h>
|
|
#include "SCServo.h"
|
|
|
|
#define SERVO_ID 1
|
|
|
|
SMS_STS sms_sts;
|
|
|
|
enum etat_t{
|
|
EN_COURS,
|
|
TERMINE,
|
|
ECHEC
|
|
};
|
|
|
|
void gestion_servo(void);
|
|
enum etat_t test_ping(void);
|
|
enum etat_t configure_servomoteur(void);
|
|
enum etat_t mouvement_servomoteur(void);
|
|
|
|
void lire_tous_les_registres(int servo_id);
|
|
|
|
// the setup routine runs once when you press reset:
|
|
void setup() {
|
|
// initialize the digital pin as an output.
|
|
pinMode(LED_BUILTIN, OUTPUT);
|
|
Serial.begin(115200);//sts舵机波特率1000000
|
|
Serial1.begin(1000000);//sts舵机波特率1000000
|
|
sms_sts.pSerial = &Serial1;
|
|
delay(5000);
|
|
|
|
}
|
|
|
|
// the loop routine runs over and over again forever:
|
|
void loop() {
|
|
digitalWrite(LED_BUILTIN, HIGH); // turn the LED on (HIGH is the voltage level)
|
|
gestion_servo();
|
|
digitalWrite(LED_BUILTIN, LOW); // turn the LED off by making the voltage LOW
|
|
gestion_servo();
|
|
}
|
|
|
|
void gestion_servo(){
|
|
static enum {
|
|
PING_SERVO,
|
|
CONFIG_SERVO,
|
|
MOUVEMENT_SERVO
|
|
} etat_gestion_servo = PING_SERVO;
|
|
|
|
enum etat_t etat_action;
|
|
|
|
switch(etat_gestion_servo){
|
|
case PING_SERVO:
|
|
if(test_ping() == TERMINE){
|
|
// etat_gestion_servo = CONFIG_SERVO;
|
|
}
|
|
break;
|
|
|
|
case CONFIG_SERVO:
|
|
etat_action = configure_servomoteur();
|
|
if(etat_action == TERMINE){
|
|
etat_gestion_servo = MOUVEMENT_SERVO;
|
|
}
|
|
if(etat_action == ECHEC){
|
|
etat_gestion_servo = PING_SERVO;
|
|
}
|
|
break;
|
|
|
|
case MOUVEMENT_SERVO:
|
|
etat_action = mouvement_servomoteur();
|
|
|
|
if(etat_action == ECHEC){
|
|
etat_gestion_servo = PING_SERVO;
|
|
}
|
|
break;
|
|
|
|
|
|
|
|
}
|
|
}
|
|
|
|
enum etat_t test_ping()
|
|
{
|
|
int ID = sms_sts.Ping(1);
|
|
static int found = 0;
|
|
if(ID!=-1){
|
|
Serial.print("Servo ID:");
|
|
Serial.println(ID);
|
|
if(found == 0){
|
|
found = 1;
|
|
lire_tous_les_registres(ID);
|
|
return TERMINE;
|
|
}
|
|
delay(100);
|
|
}else{
|
|
Serial.print("Ping servo ID error!\n");
|
|
found = 0;
|
|
delay(2000);
|
|
}
|
|
return EN_COURS;
|
|
}
|
|
|
|
enum etat_t configure_servomoteur(){
|
|
sms_sts.writeByte(SERVO_ID, SMS_STS_MODE, 3);
|
|
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, 0x0B, 0); // Buté max à 0, désactivation de la butée
|
|
sms_sts.writeByte(SERVO_ID, 0x0C, 0); // Buté max à 0, désactivation de la butée
|
|
|
|
sms_sts.writeByte(SERVO_ID, 0x1E, 3);
|
|
sms_sts.writeByte(SERVO_ID, 0x12, 0x6C); // Registre de "Phase", valeur par défaut 108 (0x6C)
|
|
|
|
|
|
return TERMINE;
|
|
}
|
|
|
|
enum etat_t mouvement_servomoteur(){
|
|
static int position=0;
|
|
static int temps_pas_ms = 0;
|
|
static int temps_aff_ms = 0;
|
|
static int pas_servo = 2048;
|
|
int position_lue;
|
|
char tampon[200];
|
|
/// Toutes les 500 ms
|
|
if(millis() - temps_pas_ms > 5000 ){
|
|
temps_pas_ms = millis();
|
|
// On avance ou recule de d'un pas
|
|
position += pas_servo;
|
|
sms_sts.WritePosEx(SERVO_ID, pas_servo, 2000);
|
|
// si position > 5000 ou position < 0
|
|
if(position > 35100 || position < 0){
|
|
pas_servo = -pas_servo;
|
|
}
|
|
|
|
}
|
|
|
|
if(millis() - temps_aff_ms > 10 ){
|
|
temps_aff_ms = millis();
|
|
sprintf(tampon, ">pos_consigne:%d\n", position);
|
|
Serial.print(tampon);
|
|
position_lue = sms_sts.ReadPos(SERVO_ID);
|
|
if(position_lue != 32769){
|
|
sprintf(tampon, ">pos_actuelle:%d\n", position_lue);
|
|
}
|
|
Serial.print(tampon);
|
|
}
|
|
|
|
return EN_COURS;
|
|
}
|
|
|
|
int lire_registre(int servo_id, int registre_adresse){
|
|
return sms_sts.readByte(servo_id, registre_adresse); // Resolution à 1
|
|
}
|
|
|
|
void lire_tous_les_registres(int servo_id){
|
|
char message[200]="";
|
|
for (int i=0; i<0x46; i++){
|
|
sprintf(message, "registre 0x%x: %d\n", i, lire_registre(servo_id, i));
|
|
Serial.print(message);
|
|
}
|
|
|
|
}
|