Hello,
I am trying to make the robot move forward when clear e.g. when the distance is more 20cm.
If the it is less than 20cm turn right.
But the code does not seem to work as intended. I have previously set if statements for distances e.g. more than 20 move forward less than 15 turn right etc. This works somewhat but i want the robot to manevour better.
Any help is welcome.
Code Below:
#include <SoftwareSerial.h> //header file of software serial port
#include <Servo.h>
#include <NewPing.h>
#include <SPI.h>
#include <nRF24L01.h>
#include <RF24.h>
#define TRIGGER_PIN A1
#define ECHO_PIN A0
#define MAX_DISTANCE 200
Servo servo;
SoftwareSerial Serial1(8,9); //define software serial port name as Serial1 and define pin8 as RX and pin9 as TX
SoftwareSerial mySerial(11, 10);
//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 = 7;
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()
{
Serial.begin(9600);
mySerial.begin(9600);
Serial1.begin(115200); //set bit rate of serial port connecting LiDAR with Arduino
servo.attach(12);
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
analogWrite(enA,170);
analogWrite(enB,170);
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("distance = ");
Serial.print(dist); //output measure distance value of LiDAR
Serial.print('\t');
Serial.print("strength = ");
Serial.print(strength); //output signal strength value
Serial.print('\n');
//Bluetooth Module - HC-05
if(Serial1.available())
{
mySerial.println(dist);
}
}
}
}
}
//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; // calculate distance in CM
delay(10);
if(distance < 20)
{
digitalWrite(in1, HIGH);
digitalWrite(in2, LOW);
digitalWrite(in3, LOW);
digitalWrite(in4, HIGH);
}
else
{
delay(40);
digitalWrite(in1, LOW);
digitalWrite(in2, HIGH);
digitalWrite(in3, LOW);
digitalWrite(in4, HIGH);
}
}