I am trying to use lidar tf luna along with servo motor but sometimes one of them or both stops working. I am a beginner and have been trying to do unit testing but I am unable to use both of them together. I would be grateful for any help, please.
#include <SoftwareSerial.h> //header file of software serial port
#include <Servo.h>
Servo servo;
SoftwareSerial Serial1(2,3); //define software serial port name as Serial1 and define pin2 as RX and pin3
int dist, dist2, dist3; //actual distance measurements of LiDAR
int strength; //signal strength of LiDAR
float temprature;
int check; //save check value
int i;
int uart[9]; //save data measured by LiDAR
const int HEADER=0x59; //frame header of data package
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
servo.attach(12);
servo.write(0);
}
void loop() {
if (Serial1.available()) { //check if serial port has data input
if(Serial1.read() == HEADER) { //assess data package frame header 0x59
uart[0]=HEADER;
if (Serial1.read() == HEADER) { //assess data package frame header 0x59
uart[1] = HEADER;
for (i = 2; i < 9; i++) { //save data in array
uart[i] = Serial1.read();
}
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
Serial.print("dist1 = ");
Serial.println(dist); //output measure distance value of LiDAR
if(dist < 100){
//Serial.println("the distance is less than 100");
servo.write(115); // resets to initial position
delay(1000);
if (Serial1.available()) { //check if serial port has data input
if(Serial1.read() == HEADER) { //assess data package frame header 0x59
uart[0]=HEADER;
if (Serial1.read() == HEADER) { //assess data package frame header 0x59
uart[1] = HEADER;
for (i = 2; i < 9; i++) { //save data in array
uart[i] = Serial1.read();
}
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
dist2 = uart[2] + uart[3] * 256; //calculate distance value
Serial.print("dist2: ");
Serial.println(dist2);
if(dist2 - dist < 20){
servo.write(140); // resets to initial position
delay(1000);
if (Serial1.available()) { //check if serial port has data input
if(Serial1.read() == HEADER) { //assess data package frame header 0x59
uart[0]=HEADER;
if (Serial1.read() == HEADER) { //assess data package frame header 0x59
uart[1] = HEADER;
for (i = 2; i < 9; i++) { //save data in array
uart[i] = Serial1.read();
}
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
dist3 = uart[2] + uart[3] * 256; //calculate distance value
Serial.print("dist3 = ");
Serial.println(dist3);
if(dist3- dist2 < 20){
Serial.println("Staircase detected");
}
}
}
}
}
}
}
}
}
}
}
if(dist > 50){
// Serial.print(dist);
Serial.println("the distance is more than 50");
}
delay(1000);
}
}
}
}
servo.write(0); // resets to initial position
delay(1000);
}