Final Project

const int trigPin = 9;
const int echoPin = A3;
long duration;
int distance;
#include <LiquidCrystal.h>
LiquidCrystal lcd(8, 7, 6, 5, 4, 3);
const int greenLed = A0;
const int redLed = 2;
#define enA 11
#define in1 12
#define in2 13
#define enB 10
#define in3 A2
#define in4 A1
int motorSpeedA = 0;
int motorSpeedB = 0;

void setup()
{
  pinMode(enA, OUTPUT);
  pinMode(enB, OUTPUT);
  pinMode(in1, OUTPUT);
  pinMode(in2, OUTPUT);
  pinMode(in3, OUTPUT);
  pinMode(in4, OUTPUT);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  lcd.begin(16, 2);
  Serial.begin(9600);
  pinMode (greenLed, OUTPUT);
  pinMode (redLed, OUTPUT);
}
void sensor ()
{
  // Reads how far away an object is from the robot (cm)
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  duration = pulseIn(echoPin, HIGH);
  distance= duration*0.034/2;
  Serial.print("Distance: ");
  Serial.println(distance);
}
void lcdWords ()
{
  // Words on the LCD and when they occur
  if (distance < 35 && distance > 5)
  {
    lcd.clear ();
    lcd.setCursor (5,0);
    lcd.print ("Excuse");
    lcd.setCursor (7,1);
    lcd.print ("Me");
    digitalWrite (redLed, HIGH);
    digitalWrite (greenLed, LOW);
    delay (500);
  }
  else if (distance < 15)
  {
    lcd.clear ();
    lcd.setCursor (6,0);
    lcd.print ("MOVE");
    delay (1000);
    lcd.setCursor (5,1);
    lcd.print ("please");
    digitalWrite (redLed, HIGH);
    digitalWrite (greenLed, LOW);
    delay (500);
  }
  else
  {
      lcd.clear();
      lcd.setCursor(5, 0);
      lcd.print("Hello");
      lcd.setCursor (3, 1);
      lcd.print ("I am Tobor");
      digitalWrite (greenLed, HIGH);
      digitalWrite (redLed, LOW);
      delay (500);
  }
}
void motors ()
{
  int xAxis = analogRead(A5);
  int yAxis = analogRead(A4);
  if (yAxis < 470) {
    // Set Motor A backward
    digitalWrite(in1, HIGH);
    digitalWrite(in2, LOW);
    // Set Motor B backward
    digitalWrite(in3, HIGH);
    digitalWrite(in4, LOW);
    // Convert the declining Y-axis readings for going backward from 470 to 0 into 0 to 255 value for the PWM signal for increasing the motor speed
    motorSpeedA = map(yAxis, 470, 0, 0, 255);
    motorSpeedB = map(yAxis, 470, 0, 0, 255);
  }
  else if (yAxis > 550) {
    // Set Motor A forward
    digitalWrite(in1, LOW);
    digitalWrite(in2, HIGH);
    // Set Motor B forward
    digitalWrite(in3, LOW);
    digitalWrite(in4, HIGH);
    // Convert the increasing Y-axis readings for going forward from 550 to 1023 into 0 to 255 value for the PWM signal for increasing the motor speed
    motorSpeedA = map(yAxis, 550, 1023, 0, 255);
    motorSpeedB = map(yAxis, 550, 1023, 0, 255);
  }
  else {
    motorSpeedA = 0;
    motorSpeedB = 0;
  }
  if (xAxis < 470) {
    int xMapped = map(xAxis, 470, 0, 0, 255);
    motorSpeedA = motorSpeedA - xMapped;
    motorSpeedB = motorSpeedB + xMapped;
    if (motorSpeedA < 0) {
      motorSpeedA = 0;
    }
    if (motorSpeedB > 255) {
      motorSpeedB = 255;
    }
  }
  if (xAxis > 550) {
    int xMapped = map(xAxis, 550, 1023, 0, 255);
    motorSpeedA = motorSpeedA + xMapped;
    motorSpeedB = motorSpeedB - xMapped;
    if (motorSpeedA > 255) {
      motorSpeedA = 255;
    }
    if (motorSpeedB < 0) {
      motorSpeedB = 0;
    }
  }
  if (motorSpeedA < 70) {
    motorSpeedA = 0;
  }
  if (motorSpeedB < 70) {
    motorSpeedB = 0;
  }
  analogWrite(enA, motorSpeedA);
  analogWrite(enB, motorSpeedB);
}
void loop()
{
  sensor ();
  lcdWords ();
  motors ();
}