How to choose starting poistion for servos with MPU-6050

Other Hardware Sensors
1

I'm trying to control servomotors by using (gyrosensor) MPU-6050. It actually works fine, but the problem is that everytime I connect a servo to the sensor, the servo rotates all the way (to its most "extreme" position). What I would like it to do is for the servo to move to its center position when connected to MPU-6050. Do you guys have any idea how to do this. This is the code I'm using. (source for the code)

#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"
#include <Servo.h>

Servo servo1;
Servo servo2;
Servo servo3;

MPU6050 accelgyro;

int16_t ax, ay, az;
int16_t gx, gy, gz;

//Usado para calcular o angulo - Variaveis do acelerometro
double accXangle;
double accYangle;
double accZangle;
//Usado para calcular o angulo - Variaveis do giroscopio
double gyroXangle = 180;
double gyroYangle = 180;
double gyroZangle = 180;

uint32_t timer;

void setup() {

Wire.begin();

// Inicializando a comunicação serial
// funciona em 8MHz ou em 16MHz
Serial.begin(38400);

// Iniciando dispositivos
Serial.println("Inicializando cominicação I2C...");

accelgyro.initialize();

// Testando a conexão com a MPU6050
Serial.println("Testando a conexão com MPU6050...");
Serial.println(accelgyro.testConnection() ? "MPU6050 conectada com sucesso" : "Falha na conexão com a MPU6050");

servo1.attach(12);
servo2.attach(10);
servo3.attach(9);

timer = micros();
}

void loop() {
// Fazendo a leitura de conexão com a MPU6050
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

// Calcular os angulos com base nos sensores do acelerometro
accXangle = (atan2(ax,az) + PI) * RAD_TO_DEG;
accYangle = (atan2(ay,az) + PI) * RAD_TO_DEG;
accZangle = (atan2(ax,ay) + PI) * RAD_TO_DEG;

double gyroXrate = ((double)gx / 131.0);
double gyroYrate = -((double)gy / 131.0);
double gyroZrate = -((double)gz / 131.0);

//###################### Calcular o ângulo de giro sem qualquer filtro #########################
gyroXangle += gyroXrate*((double)(micros()-timer)/1000000);
gyroYangle += gyroYrate*((double)(micros()-timer)/1000000);
gyroZangle += gyroZrate*((double)(micros()-timer)/1000000);

servo1.write(gyroYangle);
servo2.write(gyroXangle);
servo3.write(gyroZangle);

timer = micros();
// A taxa de amostras máxima do acelerometro é de 1KHz
delay(1);

// //Tabela Separada para os valores accel/gyro x/y/z values
// Serial.print("a/g:\t");
// Serial.print(ax); Serial.print("\t");
// Serial.print(ay); Serial.print("\t");
// Serial.print(az); Serial.print("\t");
// Serial.print(gx); Serial.print("\t");
// Serial.print(gy); Serial.print("\t");
// Serial.println(gz);

//Angulo Giroscopio x/y/z
Serial.print(gyroXangle); Serial.print("\t");
Serial.print(gyroYangle); Serial.print("\t");
Serial.print(gyroZangle); Serial.print("\t");

Serial.print("\n");
}

2

If you are attempting to power the servos from the Arduino, don't. Use a separate power supply and connect the grounds together.

3

I'm working on a similar project using a BNO055 sensor.

One important thing I've learned from others on this forum is to set the servo's position before attaching the servo.

Here's some code from my setup function.

void setup(void)
{

  for (int i = 0; i < SERVOS_IN_USE; i++)
  {
    gimbalServo[i].writeMicroseconds(INITIAL_POSITION[i]);
    gimbalServo[i].attach(SERVO_PIN[i]);
  }

Here are some of the applicable constants.

const int SERVOS_IN_USE = 3;  
const int FIRST_SERVO_PIN = 2;
const int SERVO_PIN[] = {FIRST_SERVO_PIN, FIRST_SERVO_PIN + 1, FIRST_SERVO_PIN + 2};
const long CENTER = 1500;                 // User changeable.
const long INITIAL_POSITION[] = {CENTER, CENTER, CENTER};

I've done a bit of programming to smoothly control servos from input from joysticks. I think my latest programs work really well for this purpose if I do say so myself. I have the servos accelerate as they begin to move and as they reach their target destination. This eliminates the harsh jerking motion often seen in servo projects. I encourage you to try out the code embedded in a couple of posts in this thread.

I think there are parts of the program which could be useful in your project.

BTW, I think the "write()" command is an awful way to control servos. You can achieve much better control of servos using the "writeMicroseconds()" command.

4

The Servo library will convert any write command over 500 to writeMicroSeconds, for example you can say:

servo.write(1500);

for center position.

5

outsider:
The Servo library will convert any write command over 500 to writeMicroSeconds, for example you can say:

servo.write(1500);

for center position.

That's a handy tip. Thanks for that. I hadn't delved that deeply into the Servo library.
(Gotta be worth a point. :slight_smile: )

6

DuagneDegn, can you post the actual code that you have for centering the servos. I tried it but couldn't get it to work. BUT the solution to my problem was actually very simple. I just had to change the numeral value of the gyro angle in this part of the code:

"//Usado para calcular o angulo - Variaveis do giroscopio
double gyroXangle = 180;
double gyroYangle = 180;
double gyroZangle = 180;"

The value here is the starting angle. But I would still like to see how to do it your way. Also the sensor BNO055 looks awesome! https://youtu.be/VBxMamMVSek?t=29

The Servo library will convert any write command over 500 to writeMicroSeconds, for example you can say:

servo.write(1500);

Thanks, very useful

7

jremington:
If you are attempting to power the servos from the Arduino, don't. Use a separate power supply and connect the grounds together.

That is what I'm doing. Otherwise the servos are going on and off all the time

8

realhumanbean:
DuagneDegn, can you post the actual code that you have for centering the servos. I tried it but couldn't get it to work.

I provided a link to the code in my earlier post.

Here's part of the setup code.

  for (int i = 0; i < SERVOS_IN_USE; i++)
  {
    SERVO_PULSE_RANGE[i] = MAX_PULSE[i] - MIN_PULSE[i];
    servoPosition[i] = MIN_PULSE[i] + (SERVO_PULSE_RANGE[i] / 2);
    servoSpeed[i] = 0;
    myServo[i].writeMicroseconds(servoPosition[i]); // start servo in center position
    myServo[i].attach(SERVO_PIN[i], MIN_PULSE[i], MAX_PULSE[i]);

  }

I have the servos start halfway between their minimum and maximum positions.

I'll say again, you'll get much smoother action from your servos is you use microseconds rather than angles. Even though you're using a double to hold the angle, the "write" command will truncate it to an integer value. Rather than getting over a thousand possible positions, you're only getting 181 possible positions.

It's very unlikely the degree amount is correct. There are very few servos which move exactly 180 degrees between their extremes. The relationship between pulse length and angle depends on which model of servo is being used. I measure this value and using different conversion factors depending on which servos I'm using.

HobbyKing's HXT900 servos have a 2080us pulse difference between 0 degrees and 180 degrees.

Here's a section from the constant declaration section of my program.

const float US_PER_PI = 2080.0; // 4160 per tau
const float US_PER_DEGREE[] = {US_PER_PI * 180.0 / PI, US_PER_PI * 180.0 / PI, US_PER_PI * 90.0 / PI};

"US_PER_DEGREE[]" is an array of conversion factors to change from an angle to a pulse length. If you're only using one kind of servo in your project, this doesn't need to be an array. My "yaw" servo is actually two servos so I can rotate the contraption a full 360 degrees. Since I'm using two servos together, I need to use a different conversion factor. This is why I'm using an array to hold these values since they're not all the same.

So if I know the angle I want, to find the pulse length used to position the servo is found with the equation:

  pulseLength[i] = (angle[i] * US_PER_DEGREE[i]) + MIN_PULSE[i];

I use this value with "writeMicroseconds" (or, as I recently learned, it can be used with "write") to set the servo to the desired position.

Using the equation above to find the pulse length yourself gives a more accurate (and precise) position than assuming the Arduino will make the conversion for you.

Another way to smooth out the motion of servos is to ramp their speed. The code I linked to earlier uses an acceleration value to limit how much the servos' speed can change. The code also limits the speed so the servo has time to stop with the same (magnitude of) acceleration.

Ramping the speed eliminates the jerky motion often seen in servo projects.