Code:
#include <SoftwareSerial.h> //header file of software serial port
#include <Servo.h>
#include <NewPing.h>
#include <LiquidCrystal.h>
#define TRIGGER_PIN A1
#define ECHO_PIN A0
#define MAX_DISTANCE 200
const int rs = 11, en = 12, d4 = 16, d5 = 17, d6 = 18, d7 = 19;
LiquidCrystal lcd(rs, en, d4, d5, d6, d7);
Servo servo;
SoftwareSerial Serial1(8,9); //define software serial port name as Serial1 and define pin8 as RX and pin9 as TX
//Ultrasonic Variables
NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);
boolean object;
float time = 0;
long duration, cm;
int distance = 0; //set distance value to 0
int pos = 0;
// Motor A connections
int enA = 12;
int in1 = 5;
int in2 = 6;
// Motor B connections
int enB = 2;
int in3 = 3;
int in4 = 4;
// Lidar Variables
int dist; //actual distance measurements of LiDAR
int strength; //signal strength of LiDAR
int check; //save check value
int i;
int uart[11]; //save data measured by LiDAR
const int HEADER=0x59; //frame header of data package
void setup()
{
lcd.begin(16, 2);
Serial.begin(9600);
Serial1.begin(115200); //set bit rate of serial port connecting LiDAR with Arduino
servo.attach(10);
pinMode(in1, OUTPUT);
pinMode(in2, OUTPUT);
pinMode(in3, OUTPUT);
pinMode(in4, OUTPUT);
pinMode(enA, OUTPUT);
pinMode(enB, OUTPUT);
pinMode(TRIGGER_PIN,OUTPUT); //sets TRIGGER_PIN to OUTPUT
pinMode(ECHO_PIN,INPUT); //sets ECHO_PIN to INPUT
digitalWrite(in1, LOW);
digitalWrite(in2, LOW);
digitalWrite(in3, LOW);
digitalWrite(in4, LOW);
}
void loop()
{
//Lidar Code - TF-Mini
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
strength = uart[4] + uart[5] * 256; //calculate signal strength value
// Serial Print
Serial.print("Distance = ");
Serial.print(dist); //output measure distance value of LiDAR
Serial.print('cm');
Serial.print("Strength = ");
Serial.print(strength); //output signal strength value
Serial.print('\n');
//LCD Print
lcd.setCursor(0, 0);
lcd.print("Distance: ");
lcd.print(dist);
lcd.print(" cm");
lcd.setCursor(0, 1);
lcd.print("Strength: ");
lcd.print(strength);
}
}
}
}
//Get Ultrasonic Distance
float duration, distance;
digitalWrite(TRIGGER_PIN, LOW);
delayMicroseconds(2);
digitalWrite(TRIGGER_PIN, HIGH);
delayMicroseconds(10);
digitalWrite(TRIGGER_PIN, LOW);
duration = pulseIn(ECHO_PIN, HIGH);
distance = (duration / 2) / 29.1 ; // calculate distance in CM
if(distance >= 25) //Move Forward
{
digitalWrite(in1, LOW);
digitalWrite(in2, HIGH);
digitalWrite(in3, HIGH);
digitalWrite(in4, LOW);
analogWrite(enA, 140); //enA speed variable - Right Wheel
analogWrite(enB, 140); //enB speed variable - Left Wheel
delay(200);
}
if(distance < 23) //Move Right
{
digitalWrite(in1, HIGH);
digitalWrite(in2, LOW);
digitalWrite(in3, HIGH);
digitalWrite(in4, LOW);
analogWrite(enA, 170); //enA speed variable - Right Wheel
analogWrite(enB, 140); //enB speed variable - Left Wheel
delay(200);
}
if(distance < 20) //Move Left
{
digitalWrite(in1, LOW);
digitalWrite(in2, HIGH);
digitalWrite(in3, LOW);
digitalWrite(in4, HIGH);
analogWrite(enA, 140); //enA speed variable - Right Wheel
analogWrite(enB, 170); //enB speed variable - Left Wheel
delay(200);
}
if(distance < 10) //Move Backward
{
digitalWrite(in1, HIGH);
digitalWrite(in2, LOW);
digitalWrite(in3, LOW);
digitalWrite(in4, HIGH);
analogWrite(enA, 140); //enA speed variable - Right Wheel
analogWrite(enB, 140); //enB speed variable - Left Wheel
delay(200);
}
}
