Servo Motor Rotation From Distance Input

Hi all, I am trying to get an SG90 servo motor to respond to a serial distance input where it will go to a position of 90 degrees when the distance is less than some amount and go to zero when the distance is greater than that amount. I am having an issue where the motor is jittering in place when it reaches 90 or stays at 0.

I am using an Arduino Uno board.

#include <SoftwareSerial.h>   //header file of software serial port
#include<Servo.h> // Servo Library
SoftwareSerial Serial1(3, 5); //define software serial port name as Serial1 and define pin3 as RX & pin5 as TX

// Define TFMini Variables
int dist;                     //actual distance measurements of LiDAR in cm
int strength;                 //signal strength of LiDAR
int check;                    //save check value
int i;
int uart[9];                   //save data measured by LiDAR
const int HEADER = 0x59;      //frame header of data package

// Define Servo Motor Control and Variables
Servo myservo;  //Creating servo object to control the servo
int pos=0;  // Angular position of servo motor

// TFMini Setup & Measurement Loop
void setup()
  Serial.begin(9600);         //set bit rate of serial port connecting Arduino with computer
  Serial1.begin(115200);      //set bit rate of serial port connecting LiDAR with Arduino
  myservo.attach(6);      //Attaches servo object to pwm pin 6
void loop() {
  if (Serial1.available())                //check if serial port has data input
    if ( == HEADER)        //assess data package frame header 0x59
      uart[0] = HEADER;
      if ( == HEADER)      //assess data package frame header 0x59
        uart[1] = HEADER;
        for (i = 2; i < 9; i++)         //save data in array
          uart[i] =;
        check = uart[0] + uart[1] + uart[2] + uart[3] + uart[4] + uart[5] + uart[6] + uart[7];
        if (uart[8] == (check & 0xff))        //verify the received data as per protocol
          dist = uart[2] + uart[3] * 256;     //calculate distance value
          strength = uart[4] + uart[5] * 256; //calculate signal strength value
          Serial.print("dist = ");
          Serial.print(dist);                 //output measure distance value of LiDAR
          Serial.print("strength = ");
          Serial.print(strength);             //output signal strength value
          if (dist <= 15){
          if (dist>15){


First things first… drop the Software Serial port back to less than 19200.
It’s just not reliable (or designed) any faster than that.

Thanks for your response. Just to be sure, you're referring to the Serial1.begin() number, correct? If I drop it below what it is currently, I no longer get the distance measurement in the serial monitor.

Yes, 115200 is way beyond what software serial can do reliably.

The errors mean you’ll either have to set the device to also use a lower speed, or switch to a different Arduino that has more than one hardware serial port.
(Obvious answer is a MEGA2560 - 4 ports, still pretty cheap.)

Okay, cool, thanks! Any input on the issue with the servo motor?

It could be the servo power supply, there's no other obvious reason it should be getting varying pulse widths (jitter) - it's one or the other (0/90)

P.S. Just to simplify the code, you could ...

if (dist <= 15){
} else {

Thanks for the simplification! I have it connected to the 5V terminal of the power supply board. The motor is also making a clicking noise while idle. It is also now not moving/responding to the distance measurement...

What’s the 5V supply rated for? (Amps output)

700mA. It's the default power supply module that came with the arduino

That could be an issue…
The SG90 stall is around 400mA by the spec sheet (possibly more), add the other modules, and a dubious chinese power supply, it might be getting close to the edge.

Not guaranteed, but if you can feed the servo separately, or try a beefier supply - things might get better.

P.S. don’t supply the servo from the 5V pins on the Arduino, wire it directly from the power source.

You could also use a Leonardo. Same board size and pinning as your UNO, but the serial line on pins 0/1 is not used by the USB connection, so you can use it freely.

Software serial produces lots of interrupst - especially with high baudrates - and the servo library also is based on interrupts. This may interfere, so that the ISR generated pulses of the servo lib my vary a little bit in length - which gives a jitter to the servo.

That sounds like gears jumping or broken due to overtravel.

Uh oh, do you know of a way I can test to see if it's broken?

Interesting. What I gather from this is that it's just an unfortunate side effect of using a servo motor with serial input. Is that correct? Do you know of anything I can do about it?

The servo is the only thing being powered by the 5V supply. I'm not sure what else would be interfering...

Not serial input in general, but especially SoftwareSerial.

That seems to be wrong - as I saw now it doesn't produce more interrupts than HW serial, but it disables interrupts completely durig sending/receiving a whole byte - which delays the servo lib ISR's.

Try to detach() that is switch off the servo PWM after each movement.
This could avoid jittering in it’s end positions.

Before the next movement you have to attach() it again.

This approach is working fine then there is no heavy load on the servo leverage.
Otherwise the servo would change it’s position after detaching.

Thanks for the suggestion. I tried the code below, with no luck. The motor doesn't move at all.

if (dist <= 30){
          if (dist>30){

You don't give the servo time to move.

Maybe it is easier to use my MobaTools library to drive the servo. The MoToServo class is widely compatible to the standard servo library, but it allows to define the speed of the servo and to automatically switch off pulses if the servo reaches its target position.

The servo needs some time to move to its target position.
Try to add a little delay() before detaching (e.g. 1500 ms - depends on your servo runtime) or use a corresponding "blink without delay using millis()" approach.