07/14/2023

Self-Driving Robot Complete Code

Writing about Self-Driving Robot Complete Code for educational and informational purposes only. however, do not hesitate to use this information on your own risk as we make no warranty of any kind.

Code for my self-driving robot. Feel free to use it however you like.

const int trigPin = 7; // Trigger Pin
const int echoPin = 6; // Echo Pin
const long dist = 7; // The blocker distance

#include 

Servo turnServo;  // The turning servo
Servo distanceServo; // The distance sensor servo

const int servoPin = 9;      // The turning servo
const int distancePin = 10;  // The distance sensor servo
const int distEnablePin = 4; // The distance sensor Enable

// Motor pins
const int motorEnablePin = 8;
const int motorPin1 = 3;
const int motorPin2 = 5;

void setup() {
  turnServo.attach(servoPin);
  distanceServo.attach(distancePin);
  // initialize digital pin LED_BUILTIN and motor
  pinMode(LED_BUILTIN, OUTPUT);
  pinMode(motorEnablePin, OUTPUT);
  pinMode(motorPin1, OUTPUT);
  pinMode(motorPin2, OUTPUT);
  // This is the distance sensor pins
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(distEnablePin, OUTPUT);
}

void loop() {

  digitalWrite(motorEnablePin, HIGH); //Enable Motor
  digitalWrite(distEnablePin, HIGH); //Enable Sensor

  delay(10); 
  TurnMiddle();
  delay(10); 

  int distCenter = measureDistance(55); //55 Center
  delay(100);
  int distLeft = measureDistance(110); //110 Left
  delay(100);
  int distRight = measureDistance(20); //20 Right
  delay(100);
  int minDist = min(distCenter, min(distLeft, distRight));
  
  if (minDist < dist)
  {
    digitalWrite(LED_BUILTIN, HIGH);   // turn the LED On
    delay(10); 
    TurnMiddle(); 
    delay(100);
    Stop();  
    delay(100);
    TurnLeft();
    delay(100); 
    Reverse(); 
    delay(500);
    TurnRight();
    delay(100); 
    Forward(); 
    delay(300);
  }
  else 
   {
    digitalWrite(LED_BUILTIN, LOW);   // turn the LED off and go forward
    delay(100);  
    TurnMiddle();
    Forward(); 
    delay(15);
   }
  
   
}


// Function to measure distance using the distance Sensor
int measureDistance(int servoPosition) {

  distanceServo.write(servoPosition);
  delay(200); //Wait for the sensor to reach position
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  long duration = pulseIn(echoPin, HIGH);
  int inches = duration / 74 / 2;

  return inches;
}

// Function to continue moving Forward
void Forward() 
{
  analogWrite(motorPin2, 153);
  analogWrite(motorPin1, LOW);         
}

// Function to continue moving Backward
void Reverse() 
{
  analogWrite(motorPin2, LOW);
  analogWrite(motorPin1, 153);          
}

// Function to Stop
void Stop() 
{ 
  analogWrite(motorPin2, LOW);
  analogWrite(motorPin1, LOW);         
}

void TurnRight()
{
  turnServo.write(110);
}

void TurnLeft()
{
  turnServo.write(60);
}

void TurnMiddle()
{
  turnServo.write(80);
}