hi forum i build 4wd with Arduino mega and three ultrasonic sensor. first the car goes forward and able to stop when ever meet the obstacle and move right and left alternately (jitter) without able to decide go right or left and then the car go forward although there is an obstacle resulting the car collide with object in the front of the car. thank you for any help
#include <NewPing.h> //add Ultrasonic sensor library
#define TRIGGER_PIN 40 //front
#define ECHO_PIN 41 //
#define TRIGGER_PIN3 50 // LEftSide
#define ECHO_PIN3 51 //
#define TRIGGER_PIN4 23 // RightSide
#define ECHO_PIN4 24
#define MAX_DISTANCE 170 // sets maximum useable sensor measuring distance y
NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);
NewPing sonar3(TRIGGER_PIN3, ECHO_PIN3, MAX_DISTANCE);
NewPing sonar4(TRIGGER_PIN4, ECHO_PIN4, MAX_DISTANCE);
//Servo servo_motor;
#define ENA 6 //tim4
#define IN1 7 // tim3
#define IN2 8 // tim3
#define ENB 5 //tim4
#define IN3 9 //tim3
#define IN4 10 //tim3
int CurDist = 170;
int FrontDist = 0;
int RightDist = 0;
int LeftDist = 0;
void setup() {
pinMode(ENA, OUTPUT);
pinMode(IN1, OUTPUT);
pinMode(IN2, OUTPUT);
pinMode(ENB, OUTPUT);
pinMode(IN3, OUTPUT);
pinMode(IN4, OUTPUT);
}
void loop() {
LeftDist = sonar3.ping_cm();
RightDist = sonar4.ping_cm();
if (RightDist == 0 || LeftDist == 0){
RightDist = 170;
LeftDist = 165;
}
FrontDist = sonar.ping_cm();
if (FrontDist == 0){
FrontDist = CurDist;
}
if (FrontDist <= 40){
brake();
delay(2);
RightDist = sonar4.ping_cm();
delay(33);
LeftDist = sonar3.ping_cm();
delay(33);
if (LeftDist > RightDist){
GoLeft();
}
else{
GoRight();
}
}
else
{
Forward();
}
}
void GoRight(){
turnRight();
delay(50);
}
void GoLeft(){
turnLeft();
delay(50);
}
void Forward()
{
motorAForward();
motorBForward();
}
void Backward()
{
motorABackward();
motorBBackward();
}
void turnLeft()
{
motorABackward();
motorBForward();
}
void turnRight()
{
motorAForward();
motorBBackward();
}
void coast()
{
motorACoast();
motorBCoast();
}
void brake()
{
motorABrake();
motorBBrake();
}
////////////////////////////////MOTOR A CONTROL
void motorAForward()
{
digitalWrite(IN1, LOW);
digitalWrite(IN2, HIGH);
analogWrite(ENA, 150);
}
void motorABackward()
{
digitalWrite(IN1, HIGH);
digitalWrite(IN2, LOW);
analogWrite(ENA, 150);
}
////////////////////////////////MOTOR B CONTROL
void motorBForward()
{
digitalWrite(IN3, LOW);
digitalWrite(IN4, HIGH);
analogWrite(ENB, 150);
}
void motorBBackward()
{
digitalWrite(IN3, HIGH);
digitalWrite(IN4, LOW);
analogWrite(ENB, 150);
}
//////////////////////////////////////////COASTING AND BREAKING
void motorACoast()
{
digitalWrite(IN1, LOW);
digitalWrite(IN2, LOW);
}
void motorABrake()
{
digitalWrite(IN1, HIGH);
digitalWrite(IN2, HIGH);
}
void motorBCoast()
{
digitalWrite(IN3, LOW);
digitalWrite(IN4, LOW);
}
void motorBBrake()
{
digitalWrite(IN3, HIGH);
digitalWrite(IN4, HIGH);
}