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

/*
 * Include all libraries.
*/
//Include the Arduino Stepper Library
#include <Stepper.h>

/*
 * -------------------------------------------------------
*/

/*
 * All Definitions.
*/
// Number of steps per internal motor revolution 
#define STEPS_PER_REV 32

#define GEAR_RED 64
/*
 * -------------------------------------------------------
*/

// defines pins numbers
const int trigPin = 12;
const int echoPin = 9;
//const int response = 13;
/*
 * For Motor Driver Pis.
*/
const int in1 = 2;
const int in2 = 4;
const int in3 = 7;
const int in4 = 8;
const int rightSpeed = 5;
const int leftSpeed = 3;

/*
 * For Stepper Motor Pins
*/
const int in1_1 = 6;
const int in1_2 = 10;
const int in1_3 = 11;
const int in1_4 = 13;

const int stepper_motor_speed = 700;

// Number of steps per geared output rotation
const float STEPS_PER_OUT_REV = STEPS_PER_REV * GEAR_RED;
const int StepsRequired  =  STEPS_PER_OUT_REV / 4;

// define Stepper
Stepper steppermotor(STEPS_PER_REV, in1_1, in1_2, in1_3, in1_4);

// defines variables
long duration;
int distance, distanceCm, pos;
/*
 * 
*/
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 next_step()
{
  if(pos == 1)
  {
    steppermotor.setSpeed(stepper_motor_speed);
    steppermotor.step(StepsRequired);
    pos = 2;
  }
  else if(pos ==2)
  {
    steppermotor.setSpeed(stepper_motor_speed);
    steppermotor.step(-1*StepsRequired);
    pos = 3;
  }
  else if(pos == 3)
  {
    steppermotor.setSpeed(stepper_motor_speed);
    steppermotor.step(-1*StepsRequired);
    pos = 4;
  }
  else if(pos == 4)
  {
    steppermotor.setSpeed(stepper_motor_speed);
    steppermotor.step(StepsRequired);
    pos = 1;
  }
}

/*
 * Distance meter via Ultrasonic
 */

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 turn(){
  if (pos == 1){
    turnLeft();
  } else if (pos == 2){
    absolute();
  } else if (pos == 3){
    turnRight();
  } else if (pos == 4){
    absolute();
  }
}

void loop() {
  // Calculating the distance
  distance= getDistance(echoPin, trigPin);
  
  // Prints the distance on the Serial Monitor
  //Serial.print("Distance: ");
  Serial.println(distance);
  forward();
  if (distance < 9){
    motor_stop();
    turn();
    delay(1500);
    forward();
  }
  delay(500);
}