Problems to move a servo it's whole range

Projects Programming
1

I'm using the MPU6050 to detect the pitching angle of an experimental aircraft, depending of the angle, the elevator moves up or down. This is possible using a servo of 180 degrees but I'm unable to understand why it only moves about 90 degrees.

this is the code

/*==================================
        FUENTE EXTERNA 6.4 V
//==================================
====================================
             SERVO
====================================
SNG-----PIN 8
GND-----PROTO GND-----BOARD GND
VCC-----VCC PROTO
====================================
              IMU
====================================
GND-----GND
VCC-----VCC
SDA-----Pin A4
SCL-----Pin A5
ADO-----GND
*/
// Librerias I2C para controlar el mpu6050
// la libreria MPU6050.h necesita I2Cdev.h, I2Cdev.h necesita Wire.h
#include "I2Cdev.h"
#include "MPU6050.h"
#include "Wire.h"
#include "Servo.h"
// La dirección del MPU6050 puede ser 0x68 o 0x69, dependiendo 
// del estado de AD0. Si no se especifica, 0x68 estará implicito

/// Definir los pines de entrada y salida
const int servoPin = 8;  // Pin de salida del servomotor
// La dirección del MPU6050 puede ser 0x68 o 0x69, dependiendo 
// del estado de AD0. Si no se especifica, 0x68 estará implicito
const int mpuAddress = 0x68;  // Puede ser 0x68 o 0x69
MPU6050 mpu(mpuAddress);

// Definir las constantes del PID
const double Kp = 0.5;
const double Ki = 0.2;
const double Kd = 0.1;

// Datos de la IMU
int16_t ax, ay, az; //aceleraciones
int16_t gx, gy, gz; //rotaciones

//Cnsideraciones para obtenerl los valores de la IMU
long tiempo_prev;
float dt;
float ang_x, ang_y;
float ang_x_prev, ang_y_prev;

// Definir variables para el PID
double setpoint = 0;
double PWM;
double input, output;
double error, lastError = 0;
double integral = 0, derivative;

// Definir el objeto Servo
Servo myservo;
float aero;
double tp =1500; //posicion deseada del timon (el servo trabaja con 1500 en 0 grados, 2000 en 90 grados y 1000 en -90 grados)


void setup()
{
	Serial.begin(9600);
	Wire.begin();
	mpu.initialize();
  myservo.attach(servoPin);
  myservo.write(0); //posicion inicial del servo en el centro
  if(mpu.testConnection()){
    Serial.println("IMU iniciado correctamente");
  }
  else{
    Serial.println("ERROR");
  }
}

void loop() 
{
	// Leer las aceleraciones y velocidades angulares
	mpu.getAcceleration(&ax, &ay, &az);
	mpu.getRotation(&gx, &gy, &gz);

	dt = (millis() - tiempo_prev) / 1000.0;
	tiempo_prev = millis();
	//Calcular los ángulos con acelerometro
	float accel_ang_x = atan(ay / sqrt(pow(ax, 2) + pow(az, 2)))*(180.0 / 3.14);
	float accel_ang_y = atan(-ax / sqrt(pow(ay, 2) + pow(az, 2)))*(180.0 / 3.14);

	//Calcular angulo de rotación con giroscopio y filtro complementario
	ang_x = 0.98*(ang_x_prev + (gx / 131)*dt) + 0.02*accel_ang_x;
	ang_y = 0.98*(ang_y_prev + (gy / 131)*dt) + 0.02*accel_ang_y;

	ang_x_prev = ang_x;
	ang_y_prev = ang_y;
  //Imprimir los valores del angulo de rotacion en x
	Serial.print(F("Rotacion en X:  "));
	Serial.print(ang_x);
  Serial.print("\n");
  //Calcular el error
  input = ang_x;
  error = setpoint - input;
  if(setpoint > error)
  {
  // Calcular la integral y derivada del error
  integral += error;
  derivative = error - lastError;
  // Calcular la salida PWM utilizando PID 
  PWM = Kp * error + Ki * integral + Kd * derivative;
  output = tp+PWM;
  }
  if (setpoint < error)
  {
  // Calcular la integral y derivada del error
  integral += error;
  derivative = error - lastError;
  // Calcular la salida PWM utilizando PID 
  PWM = Kp * error + Ki * integral + Kd * derivative;
  output = tp-PWM;
  }
  // Limitar la salida entre -90 y 90 grados
  if(output < 1000) {
    output = 1000;
  } else if(output > 2000) {
    output = 2000;
  }
  //Mandar al servo la deflexion correspondiente
  myservo.write(output);
  lastError = error;
  //Retraso minimo capaz de ser usado = 20 ms
	delay(20);
}
2

It looks like you are confused about the Servo functions

write() takes an angle as its parameter but you seem to be using microseconds. If so, consider using the writeMicroseconds() function instead

3
  • Do some Serial.print( myVariable ) on different variables to confirm they are what you think the are.
6

I know, but why not use the explicit function rather than confusing things

@andrsaltzr
Have you tried adjusting the range of values used by the servo by adding the extra parameters to the attach() function ?

Come to that, is the servo actually capable of moving through 180 degrees ?

1 Like
7

…or the constrain() function.

9

Try this test program to determine the movement limits of your servo:

/*
 Try this test sketch with the Servo library to see how your
 servo responds to different settings, type a position
 (0 to 180) or if you type a number greater than 180 it will be
 interpreted as microseconds(544 to 2400), in the top of serial
 monitor and hit [ENTER], start at 90 (or 1472) and work your
 way toward zero (544) 5 degrees (or 50 micros) at a time, then
 toward 180 (2400). 
*/
#include <Servo.h>
Servo servo;

void setup() {
  // initialize serial:
  Serial.begin(9600); // set serial monitor baud rate to match
                      // set serial monitor line ending to "NewLine"
  servo.write(90);
  servo.attach(8);
  prntIt();
}

void loop() {
  // if there's any serial available, read it:
  while (Serial.available() > 0) {

    // look for the next valid integer in the incoming serial stream:
    int pos = Serial.parseInt();
    if(Serial.read() == '\n'){} //skip 1 second delay
    pos = constrain(pos, 0, 2400);
    servo.write(pos);
    prntIt();
  }
}
void prntIt()
{
  Serial.print("  degrees = "); 
  Serial.print(servo.read());
  Serial.print("\t");
  Serial.print("microseconds =  ");
  Serial.println(servo.readMicroseconds());
}
1 Like
11

Move the linkage point either on the elevator horn or the servo horn.
Further out on the servo horn will give more elevator travel, further in on the elevator horn will do the same.
.........and obviously visa versa.

12

TNX for that.

13

This topic was automatically closed 180 days after the last reply. New replies are no longer allowed.