/*
* Athors: Oulis Evnagelos, Oulis Nikolaos, Katsibras Drosos
* Ultrasonic Sensor HC-SR04 and Arduino
* Dual Stepper Motor Shiled 1.1
*
*/

// defines pins numbers
const int trigPin = 10;
const int echoPin = 9;
const int response = 12;
const int in1 = 2;
const int in2 = 4;
const int in3 = 7;
const int in4 = 8;
const int rightSpeed = 5;
const int leftSpeed = 3;

// defines variables
long duration;
int distance;

void clearUltrasonic(){
  // Clears the trigPin
  analogWrite(trigPin, LOW);
  delayMicroseconds(2);
}

void setup() {
pinMode(trigPin, OUTPUT); // Sets the trigPin as an Output
pinMode(echoPin, INPUT); // Sets the echoPin as an Input

pinMode(response, OUTPUT); // Sets to led aoutput.

pinMode(in1, OUTPUT); //Motor Driver 1st Pin
pinMode(in2, OUTPUT); //Motor Driver 2nd Pin
pinMode(in3, OUTPUT); //Motor Driver 3rd Pin
pinMode(in4, OUTPUT); //Motor Driver 4th Pin

pinMode(rightSpeed, OUTPUT);
pinMode(leftSpeed, OUTPUT);

Serial.begin(9600); // Starts the serial communication
clearUltrasonic();
}

//Stepper Function
void step(/*boolean dir,*/int steps){
//  digitalWrite(dirPin1,dir);
//  digitalWrite(dirPin2,dir);
  delay(50);
  for(int i=0;i<steps;i++){
    forward();
    delayMicroseconds(100);
    motor_stop();
    delayMicroseconds(100);
  }
}


void turnLeft(){
  analogWrite(leftSpeed, 255);
  analogWrite(rightSpeed, 0);
}

void turnRight(){
  analogWrite(leftSpeed, 0);
  analogWrite(rightSpeed, 255);
}

void absolute(){
  analogWrite(leftSpeed, 255);
  analogWrite(rightSpeed, 255);
}

void backward(){
  // Set Motor A backward
  digitalWrite(in1, HIGH);
  digitalWrite(in2, LOW);
  // Set Motor B backward
  digitalWrite(in3, HIGH);
  digitalWrite(in4, LOW);
}

void forward(){
  // Set Motor A forward
  digitalWrite(in1, LOW);
  digitalWrite(in2, HIGH);
  // Set Motor B forward
  digitalWrite(in3, LOW);
  digitalWrite(in4, HIGH);
}

void motor_stop(){
  digitalWrite(in1, LOW);
  digitalWrite(in2, LOW);
  digitalWrite(in3, LOW);
  digitalWrite(in4, LOW);
}

void loop() {
// Sets the trigPin on HIGH state for 10 micro seconds
analogWrite(trigPin, HIGH);
delayMicroseconds(10);
analogWrite(trigPin, LOW);

// Reads the echbackwardoPin, returns the sound wave travel time in microseconds
duration = pulseIn(echoPin, HIGH);

// Calculating the distance
distance= duration*0.034/2;

// Prints the distance on the Serial Monitor
//Serial.print("Distance: ");
//Serial.println(distance);
absolute();
if (distance < 10){
  motor_stop();
  digitalWrite(response, HIGH);
}
else{
  forward();
  digitalWrite(response, LOW);
}
clearUltrasonic();
delay(1500);
}