Control de posición Parallax Feedback 360º con Arduino Uno

Hola buenos días,

Estoy intentado controlar desde hace unos días un servo de velocidad Parallax Feedback 360º con una placa Arduino Uno. Necesito que este gire de forma precisa de 5 en 5 grados parando un tiempo entre movimiento y movimiento.

NO TENGO NI IDEA…lo fácil sería comprarme una placa Parallax y directamente carga el programa que he encontrado en su web pero preferiría hacerlo con mi Arduino Uno.

Si me podeis ayudar encantado, he encontrado un programa que me permite mover un determinado ángulo en concreto, que ya para mi es mucho…lo que necesitaría es que fuera incrementándose de 5º en 5º, aquí os lo dejo. Gracias por adelantado, es mi primera experiencia con Arduino y mi nivel es 0.

 #include <Servo.h>            //Servo library

//PID angle control for Parallax 360 
Servo servo_test;        //initialize a servo object for the connected servo  
int pinFeedback = A0;
               
int target_angle =220;
float tHigh = 0;
float tLow = 0;
int tCycle = 0;
float dc = 0;
float angle = 0; //Measured angle from feedback
float dcMin = 2.9; //From Parallax spec sheet
float dcMax = 97.1; //From Parallax spec sheet
float Kp = .7; //Proportional Gain, higher values for faster response, higher values contribute to overshoot.
float Ki = .2; //Integral Gain, higher values to converge faster to zero error, higher values produce oscillations. Higher values are more unstable near a target_angle = 0.
float iLimit = 5; //Arbitrary Anti-wind-up
float Kd = 1; //Derivative Gain, higher values dampen oscillations around target_angle. Higher values produce more holding state jitter. May need filter for error noise.
float prev_error = 0;
float prev_pError = 0;
float error = 0;
float pError = 0;
float iError = 0;


void setup() 
{ 
 servo_test.attach(3);      // attach the signal pin of servo to pin3 of arduino
 pinMode(A0, INPUT);
} 
 
void loop() 
{ 
 //use line below to determine "center" of servo. When near values of 90, manually force servo by hand in both directions to see if it continues to turn. 
 //servo_test.write(85); //clockwise: max 0 - 90 min, counter-clockwise: min 96-180 max, may be different for your servo, 93 stopped.
 
 while(1) //From Parallax spec sheet
 {
   tHigh = pulseIn(pinFeedback, HIGH);
   tLow = pulseIn(pinFeedback, LOW);
   tCycle = tHigh + tLow;
   if ( tCycle > 1000 && tCycle < 1200)
   {
     break; //valid tCycle;
   }
 }
 
 dc = (100 * tHigh) / tCycle; //From Parallax spec sheet, you are trying to determine the percentage of the HIGH in the pulse
 
 angle = ((dc - dcMin) * 360) / (dcMax - dcMin + 1); //From Parallax spec sheet

 //Keep measured angles within bounds
 if (angle < 0)
 {
   angle = 0;
 }
 else if(angle > 359)
 {
   angle = 359;
 }

 if (target_angle < 0)
 {
   target_angle = 360 + target_angle; //handles negative target_angles;
 }
 
 error = target_angle - angle;
 
 if(error >220)
 {
   error = error - 360; //tells it to rotate in the other direction because it is a smaller angle that way.
 }
 if (error < -220)
 {
   error = 360 - error - 360; //tells it to rotate in the other direction because it is a smaller angle that way.
 }

 // PID controller stuff, Adjust values of Kp, Ki, and Kd above to tune your system
 float pError = Kp * error;
 float iError = Ki * (error + prev_error);

 if  (iError > iLimit)
 {
   iError = iLimit;
 }
 if (iError <  -iLimit)
 {
   iError = -iLimit;
 }
 
 prev_error = error;
 float dError = Kd * (pError - prev_pError);
 prev_pError = pError;

 error = error / 2; //max 180 error will have max 90 offset value

 int val =89 - (Kp * error) - iError - dError; // 93 is the middle of my servo's "no motion" dead-band
 
 servo_test.write(val); //Move the servo


                      

 
 
}

Te envié privado para que edites tu post. Por favor hazlo!! Arriba derecha se verá con un (1)

Prueba a ver como se comporta este código modificado.

#include <Servo.h>            //Servo library

//PID angle control for Parallax 360 
Servo servo_test;        //initialize a servo object for the connected servo  

#define DEMORA    5000  // 5000 mseg
unsigned long start;
int pinFeedback   = A0;
                
float target_angle= 220.0;
float tHigh       = 0;
float tLow        = 0;
int tCycle        = 0;
float dc          = 0;
float angle       = 0; //Measured angle from feedback
float dcMin       = 2.9; //From Parallax spec sheet
float dcMax       = 97.1; //From Parallax spec sheet
float Kp          = 0.7; //Proportional Gain, higher values for faster response, higher values contribute to overshoot.
float Ki          = 0.2; //Integral Gain, higher values to converge faster to zero error, higher values produce oscillations. Higher values are more unstable near a target_angle = 0.
float iLimit      = 5; //Arbitrary Anti-wind-up
float Kd          = 1; //Derivative Gain, higher values dampen oscillations around target_angle. Higher values produce more holding state jitter. May need filter for error noise.
float prev_error  = 0;
float prev_pError = 0;
float error       = 0;
float pError      = 0;
float iError      = 0;
 
void setup() { 
  servo_test.attach(3);      // attach the signal pin of servo to pin3 of arduino
  pinMode(A0, INPUT);
} 
  
void loop() { 
  // use line below to determine "center" of servo. 
  // When near values of 90, manually force servo by hand in both directions to see if it continues to turn. 
  // servo_test.write(85); 
  // clockwise: max 0 - 90 min, 
  // counter-clockwise: min 96-180 max, may be different for your servo, 93 stopped.
  
  for (int ang = 0.0; ang < 90.0; ang +=5.0) {
       start = millis();
       while (millis()-start < DEMORA) { // permanezco DEMORA mseg por posicion
              mido_angulo();
              posicionar(ang);  
       }                         
  }
}

void mido_angulo() {
  while(1) {  //From Parallax spec sheet
    tHigh  = pulseIn(pinFeedback, HIGH);
    tLow   = pulseIn(pinFeedback, LOW);
    tCycle = tHigh + tLow;
    if ( tCycle > 1000 && tCycle < 1200) {
        break; //valid tCycle;
    }
  }
  
  dc = (100 * tHigh) / tCycle; //From Parallax spec sheet, you are trying to determine the percentage of the HIGH in the pulse
  angle = ((dc - dcMin) * 360) / (dcMax - dcMin + 1); //From Parallax spec sheet

  //Keep measured angles within bounds
  if (angle < 0.0) {
      angle = 0.0;
  }
  else 
  if (angle > 359.0) {
      angle = 359.0;
  }
}
void posicionar(int target_angle){
  if (target_angle < 0.0) {
      target_angle = 360.0 + target_angle; //handles negative target_angles;
  }
  error = target_angle - angle;
  
  if (error >220.0) {
      error = error - 360; //tells it to rotate in the other direction because it is a smaller angle that way.
  }
  if (error < -220.0) {
      error = 360.0 - error - 360.0; //tells it to rotate in the other direction because it is a smaller angle that way.
  }

  // PID controller stuff, Adjust values of Kp, Ki, and Kd above to tune your system
  float pError = Kp * error;
  float iError = Ki * (error + prev_error);

  if (iError > iLimit) {
      iError = iLimit;
  }
  if (iError <  -iLimit) {
      iError = -iLimit;
  }
  
  prev_error = error;
  float dError = Kd * (pError - prev_pError);
  prev_pError = pError;

  error = error / 2; //max 180 error will have max 90 offset value
  int val = 89 - (Kp * error) - iError - dError; // 93 is the middle of my servo's "no motion" dead-band
  
  servo_test.write(val); //Move the servo 
}

Si lo hace bien, debería ir de 0 a 90 en pasos de 5.0 grados en intervalos de 5 segundos por paso.

Muchas gracias, :slight_smile: hace justo lo que dices, gira 90º en pasos de 5º, eso es lo que necesitaba.

Pero hay un problema, parece ser que por motivos de falta de par y rozamiento no consigue clavar la distancia, se te ocurre alguna forma de controlar el servo para que sea más preciso es decir que no pare de mandar energía hasta que llegue al ángulo.

De todas formas muchas gracias..

Determina cuanto demora en llegar al ángulo.
Eso lo dejé a tu experimentación.

Modifica DEMORA para que pueda llegar al valor.

Pues si, he ampliado la demora y parece que mejora aunque tendría que medirlo fisicamente.... me podrias indicar como monitorizar el ángulo, ayer estuve haciendo pruebas con unos compañeros y conseguían leer el encoder del servo y convertirlo en grados.