/*
* 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 = 12;
const int echoPin = 9;
const int response = 13;
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, distanceCm;

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();
}

int getDistance(int echoPin, int trigPin){
  clearUltrasonic();
  // Sets the trigPin on HIGH state for 10 micro seconds
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

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

  return distanceCm;
}

//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() {
// Calculating the distance
distance= getDistance(echoPin, trigPin);

// 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);
}
delay(1000);
}