//VEMMANUEL'S AND ISSAC LEGENDARY ROBOT SPINNER
/*
 * ECT242 V4 Elegoo Robot Kit interface
 * Robot Resource layout
 * bp 4/17/24 
 */
#include <arduino.h> // a standard include file

// The code that folllows is all definitions that we'll use

// Set up definitions, give names to, pins on the Arduino
#define PIN_Motor_STBY 3        // enable pin H = motors on, L = off
#define PIN_Motor_PWMA 5        // PWM for R motors (speed) 
#define PIN_Motor_PWMB 6        // PWM for L motors
#define PIN_Motor_AIN_1 7       // Dir for R motors H = forward L= backward
#define PIN_Motor_BIN_1 8       // Dir for L motors 

// Definition of motor command and performance
// 1) Both motors move in same direction and same speed -  Straight
// 2) Motors move in opposite directions and same speed - Tank Turn
// 3) Both motors move in F direction but A speed > B - Left turning
// 4) Both motors move in F direction but A speed < B - Right turning  
 
// Set up motor speed presets - PWM setting can be from 0 (off ) to 255 (max)
int speed_Zero = 0;   // PWM = 0% Duty Cycle
int speed_Min = 64;   // PWM = 25% Duty Cycle
int speed_MidL = 127; // PWM = 50% Duty Cycle
int speed_MidH = 191; // PWM = 75% Duty Cycle
int speed_Max = 255;  // PWM = 100% Duty Cycle


// Ultrasound sensor pin setting
  int Echo = 12;  
  int Trig = 13; 

// Definition of commands and performance
// 1) Trigger a PING and time the echo to find distance  

// Variables we will use  
float Fdistance = 0;    // A place to store a value of distance

// Servo motor controlls "view" of ultra sonic sensor
#include <Servo.h>          // the file with all the servo commands
Servo myEyes;           // create servo object to control a servo
int Eyes = 10;           // pin setting

long duration;

void setup() 
{
 // motor setup using resources defined above:
  pinMode(PIN_Motor_PWMA,OUTPUT);
  pinMode(PIN_Motor_AIN_1,OUTPUT);
  pinMode(PIN_Motor_PWMB,OUTPUT);
  pinMode(PIN_Motor_BIN_1,OUTPUT);
  pinMode(PIN_Motor_STBY,OUTPUT);

 digitalWrite(PIN_Motor_STBY, HIGH); //To insure that the motors are always on and I just need to lower the voice to either one
 
// Ultrasound pin setting
  pinMode(Echo, INPUT);    
  pinMode(Trig, OUTPUT); 
  Serial.begin(19200);    //Sets up a serial port to communicate with the console  

// Servo set up 
  myEyes.attach(Eyes);      // attaches the servo on pin 10 to the servo object
}

  

void loop() 

{
  // put your main code here, to run repeatedly:


digitalWrite(PIN_Motor_AIN_1, HIGH); 
analogWrite(PIN_Motor_PWMA, speed_MidL);
digitalWrite(PIN_Motor_BIN_1, HIGH); 
analogWrite(PIN_Motor_PWMB, speed_MidL);

myEyes.write(90);
digitalWrite(Trig, LOW);
delayMicroseconds(2);
digitalWrite(Trig, HIGH);
delayMicroseconds(10);
digitalWrite(Trig, LOW);
duration = pulseIn(Echo, HIGH);
int Fdistance = duration * 0.034 / 2;
Serial.println(Fdistance);
Serial.println("WORKING FORWARD");

if (Fdistance <= 200){
  digitalWrite(PIN_Motor_AIN_1, LOW);
  digitalWrite(PIN_Motor_BIN_1, LOW);
  Serial.println("WORKING");
  myEyes.write(180);
  LookingR();
  myEyes.write(0);
  LookingL();
  
}
}


void LookingR(){
if (Fdistance <= 300){
  Serial.println("WORKING RIGHT");
  digitalWrite(PIN_Motor_AIN_1, HIGH);
  analogWrite(PIN_Motor_PWMA, speed_Zero);  
  digitalWrite(PIN_Motor_BIN_1, HIGH);
  analogWrite(PIN_Motor_PWMB, speed_Min);
  delay(1000);
}
}

void LookingL(){
  if (Fdistance <= 300){
  Serial.println("WORKING LEFT");
  digitalWrite(PIN_Motor_AIN_1, HIGH);
  analogWrite(PIN_Motor_PWMA, speed_Min); 
  digitalWrite(PIN_Motor_BIN_1, HIGH); 
  analogWrite(PIN_Motor_PWMB, speed_Zero);  
  delay(1000);
  }
}