Extreme lag when controlling 4 servos with Arduino Uno

Hi! I am a total noob with Arduino, and am trying to control 4 small servo motors based off data from an MPU6050 accelerometer. My goal is this: based off the angle of the accelerometer, rotate the servo to that angle. It works very well running only one arduino, but running 4 the lag is incredible, upwards of 10 seconds in some cases. I'm not sure if this is an issue with my code, with my circuitry, or with my power supply. My circuitry setup for the MPU6050 is: VCC -> 5v, GND -> GND, SCL -> A5, SDA -> A4. Then, for the servos, I run a wire from Vin to one part of a breadboard, and a wire from Gnd to another part of the breadboard. The servo motor's grounds and power supply wires are connected in series with the breadboard. Then, the servo motor's input wire is connected to the respective pin (I am using pins 3, 5, 6, and 9). I am also using my computer as a power supply, maybe this is an issue?

Below is the code I am using, thank you for any help! Let me know if my description of the setup / code is unclear, or if a picture / video would help :slight_smile:

/*MPU6050 sensor with servo control.
   https://www.mpusensor.com
*/


// This next portion includes neccesary libraries, namely the Adafruit MPU6050 Accelerometer library, and servo libraries.
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>
#include <Servo.h>


// Objects are created for the libraries
Servo servox1;
Servo servox2;
Servo servoy1;
Servo servoy2; 
Adafruit_MPU6050 mpusensor;


// Setup Function
void setup(void) {
  Serial.begin(115200); // Serial Monitor begins - the number here is the BAUD rate, which is essentially how fast the data is to be sent or recieved. If the serial monitor is displaying weird value, make sure the baud value matches what is listed here. 
  servox1.attach(3); // Sets the servo motor PIN number on ARDUINO - pin number should correspond to a DIGITAL pin, but does not have to be a dedicated hardware PWM pin
  servox2.attach(5); // Sets the servo motor PIN number on ARDUINO - pin number should correspond to a DIGITAL pin, but does not have to be a dedicated hardware PWM pin 
  servoy1.attach(6); // Sets the servo motor PIN number on ARDUINO - pin number should correspond to a DIGITAL pin, but does not have to be a dedicated hardware PWM pin 
  servoy2.attach(9); // Sets the servo motor PIN number on ARDUINO - pin number should correspond to a DIGITAL pin, but does not have to be a dedicated hardware PWM pin 
  
  Wire.begin(); // Begin wire library
  mpusensor.begin(); // Begin MPU6050 Sensor
  servox1.write(0); // Turn the servo motor to the starting position
  servox2.write(0); // Turn the servo motor to the starting position
  servoy1.write(0); // Turn the servo motor to the starting position
  servoy2.write(0); // Turn the servo motor to the starting position

  // The next 3 lines of code set the accelerometer and gyroscope ranges
  mpusensor.setAccelerometerRange(MPU6050_RANGE_8_G);//2_G,4_G,8_G,16_G
  mpusensor.setGyroRange(MPU6050_RANGE_500_DEG);//250,500,1000,2000
  mpusensor.setFilterBandwidth(MPU6050_BAND_21_HZ);

}



// Loop Function
void loop() {

  // Get new sensor data from MPU6050
  sensors_event_t a, g, temp;
  mpusensor.getEvent(&a, &g, &temp);

  
  int y_value = a.acceleration.y; // Accelerator Y-axis values are stored in variable "y_value"
  int x_value = a.acceleration.x; // Accelerator X-axis values are stored in variable "x_value"

  
  y_value = map(y_value,  -10, 10, 180, 0); // y_value from accelerometer converted to degrees ranging from 0 to 180. 
  int opp_y_val = abs(180-y_value); // finds the opposite value of the corresponding y value
  
  x_value = map(x_value,  -10, 10, 180, 0); // x_value from accelerometer converted to degrees ranging from 0 to 180.
  int opp_x_val = abs(180-x_value); // finds the opposite value of the corresponding x value


  servoy1.write(opp_y_val); // servo motor is rotated to the opposite of the mapped y_value
  servoy2.write(opp_y_val); // servo motor is rotated to the opposite of the mapped y_value
  servox1.write(opp_x_val); // servo motor is rotated to the opposite of the mapped x_value
  servox2.write(opp_x_val); // servo motor is rotated to the opposite of the mapped x_value
  
  Serial.println(opp_y_val); // opp_y_val is displayed on the serial monitor
  Serial.println(opp_x_val); // opp_x_val is displayed on the serial monitor
  
  //delay(10); // optional delay

}

You need to use a ...WithoutDelay method. I would highly recommend starting with this tutorial on the Adafruit site. It leads into using a class for the servos that will make your life a whole lot easier. Start at the start so you understand why it's needed.

does a withoutdelay method mean something beyond not using the delay(); commands?

Yes it does, but it also conceptually takes a "serial" process and "parallelizes" it somewhat. The reason I suggested it is not so much because of delay() but because of the four servo.write() as I suspect that is your lag: update one, then another, etc, then take readings, update etc.

In hindsight, I was probably hasty as I haven't done much with multiple simultaneous servos so maybe it isn't completely appropriate in this instance

What does this output?
also you can add a loopTimer to see how fast your loop is running

Install SafeString library from the library manager and add
#include "loopTimer.h"

and then in loop() add

void loop() {
  loopTimer.check(Serial);  // will print timings every 5 sec.
 ...

that output I just put there to see the accelerometer values on the serial monitor on the arduino IDE. Thank you for the suggestions, if you don't mind me asking, what does the loopTimer.check(Serial) thing do? does it add functionality beyond printing timings?

No just a view of how fast/slow you loop is running. I tried a simple sketch without the MPU and it ran very very fast so just wondering what actual value you are updating your servos with and if they are just being push back and forth very quickly due to noise on the measurement

I think I might understand what you are suggesting, are you suggesting that instead of updating each servo's values every 'timestep', to update them every couple timesteps or one servo per timestep? to me this conceptually makes sense why it would speed up lag, since the servos are probably receiving data way faster than they can even move so many inputs are kind of useless and just causing unnecessary computation. Am I thinking about this correctly?

I think I understand, if that were the case wouldn't 1 servo also lag a lot?

Yes. If you have some idea of how fast the servos move you can do a rough calculation of how long it will take to move from the last command position to the new one and work out where it is now and if it is worth while sending a new command.
This code can get messy fast.
For progrmming the update delays see my tutorial on How to write Timers and Delays in Arduino

Sounds reasonable. I would need to play with some actual servos and your MPU to get a better idea of what is happening.

These two libraries might be of help

Thank you! These look like good resources, however, I am not sure if it would help with lag. I am thinking now that the power supply might be insufficient. I can run the same program with 4 servos connected and have it lag significantly, but if I remove 3 then the 1 connected servo runs flawlessly. I am wondering if maybe I am just splitting current in too many ways. The SG90 servos supposedly need around 600~700mA, and from what I can tell, the max expected output current from the Arduino 5v pin is around 700mA, in which case I am running the 4 servos with way less current than I am supposed to (each around 175mA). I also tried editing the code to only update the servos a given timestep using millis() (see below), which did not fix the lag but maybe helped a little. I think I am going to try giving each of the servos their own power supply (using a 9v battery and a power supply module for each servo) to see if this fixes the lag.

/*MPU6050 sensor with servo control.
   https://www.mpusensor.com
*/


// This next portion includes neccesary libraries, namely the Adafruit MPU6050 Accelerometer library, and servo libraries.
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>
#include <Servo.h>


// Objects are created for the libraries
Servo servox1;
Servo servox2;
Servo servoy1;
Servo servoy2; 
Adafruit_MPU6050 mpusensor;
uint32_t previous_time; // initialize object to store time

// Setup Function
void setup(void) {
  Serial.begin(115200); // Serial Monitor begins - the number here is the BAUD rate, which is essentially how fast the data is to be sent or recieved. If the serial monitor is displaying weird value, make sure the baud value matches what is listed here. 
  servox1.attach(3); // Sets the servo motor PIN number on ARDUINO - pin number should correspond to a DIGITAL pin, but does not have to be a dedicated hardware PWM pin
  servox2.attach(5); // Sets the servo motor PIN number on ARDUINO - pin number should correspond to a DIGITAL pin, but does not have to be a dedicated hardware PWM pin 
  servoy1.attach(6); // Sets the servo motor PIN number on ARDUINO - pin number should correspond to a DIGITAL pin, but does not have to be a dedicated hardware PWM pin 
  servoy2.attach(9); // Sets the servo motor PIN number on ARDUINO - pin number should correspond to a DIGITAL pin, but does not have to be a dedicated hardware PWM pin 
  
  Wire.begin(); // Begin wire library
  mpusensor.begin(); // Begin MPU6050 Sensor
  servox1.write(0); // Turn the servo motor to the starting position
  servox2.write(0); // Turn the servo motor to the starting position
  servoy1.write(0); // Turn the servo motor to the starting position
  servoy2.write(0); // Turn the servo motor to the starting position

  // The next 3 lines of code set the accelerometer and gyroscope ranges
  mpusensor.setAccelerometerRange(MPU6050_RANGE_8_G);//2_G,4_G,8_G,16_G
  mpusensor.setGyroRange(MPU6050_RANGE_500_DEG);//250,500,1000,2000
  mpusensor.setFilterBandwidth(MPU6050_BAND_21_HZ);

  previous_time = millis(); // store beginning time 
}



// Loop Function
void loop() {

if (millis()-previous_time >= 50) { // only perform things in loop every interval of time
  // Get new sensor data from MPU6050
    sensors_event_t a, g, temp;
    mpusensor.getEvent(&a, &g, &temp);
    
    int y_value = a.acceleration.y; // Accelerator Y-axis values are stored in variable "y_value"
    int x_value = a.acceleration.x; // Accelerator X-axis values are stored in variable "x_value"
    
    y_value = map(y_value,  -10, 10, 180, 0); // y_value from accelerometer converted to degrees ranging from 0 to 180. 
    x_value = map(x_value,  -10, 10, 180, 0); // x_value from accelerometer converted to degrees ranging from 0 to 180.
  
    servoy1.write(y_value); // servo motor is rotated to the mapped y_value
    servoy2.write(y_value); // servo motor is rotated to the mapped y_value
    servox1.write(x_value); // servo motor is rotated to the mapped x_value
    servox2.write(x_value); // servo motor is rotated to the mapped x_value
    
    Serial.println(y_value); // y_value is displayed on the serial monitor
    Serial.println(x_value); // x_value is displayed on the serial monitor
    
    previous_time = millis(); // update time
    
  } 
}```

The servos definitely need separate power - there's no way you can run four from Arduino power.

that is what I figured! I am going to try powering each from their own 9v 600mA battery (passed through a voltage limiter). I'll update this thread once I try that.

If that's a smoke detector battery, forget it - they're far too feeble. Use AAs instead.

No I'm thinking a normal rectangular 9v battery, or do you think this would not be enough? for reference, during actual use, I only need the servos to work for around 5 minutes. I don't need the batteries to last very long.

Update: Solution has been found!

More current was needed. Giving each Servo its own 9v 600mA battery fixed all issues.


Generally considered to be a smoke alarm battery.

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