Servo Motor Response From Tfmini Output Data

Hi! I'm trying to create a simple script where my servo motor moves to a 90 degree position while the distance measured by a tfmini is <=5 and moves to 0 degrees otherwise.

This is the code that I have and I'm not too sure why it's not working. The motor is only essentially jittering back and forth in place.
I'm using an arduino uno.

#include <SoftwareSerial.h>   //header file of software serial port
#include<Servo.h> // Servo Library
SoftwareSerial Serial1(0, 1); //define software serial port name as Serial1 and define pin0 as RX & pin1 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(9);    //Attaches servo object to pwm pin 9
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
          while (dist < 10);{
          while (dist>5){

Please follow the advice given in the link below when posting code, in particular the section entitled 'Posting code and common code problems'

Use code tags (the </> icon above the compose window) to make it easier to read and copy for examination

SoftwareSerial Serial1(0, 1); //define software serial port name as Serial1 and define pin0 as RX & pin1 as TX

Why are you using the hardware Serial pins with SoftwareSerial ?

Which Arduino?

Thank you! I've made a few changes.

Uno R3

For that portion, I was following a sample code that got the sensor to measure distance. I don't have any particular reason why those pins are used.

When using a Uno do not use pins 0 and 1 for SoftwareSerial. Choose another pair of pins

Which pins would you suggest?

Any of them apart from 0 and 1

Thank you!

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