/* 
 * Make motor scan back and forwards in a certain angle interval.
 * Daniel Sjoberg, 2019-01-27
 */

#define echoPin 13             // Sensor Echo Pin connected to arduino pin 13
#define trigPin 12             // Sensor Trigger Pin connected to arduino pin 12
#define powerPin 7             // Sensor power supply taken from arduino pin 7

#define motorPin1 8            // Motor pin 1 connected to arduino pin 8
#define motorPin2 9            // Motor pin 2 connected to arduino pin 9
#define motorPin3 10           // Motor pin 3 connected to arduino pin 10
#define motorPin4 11           // Motor pin 4 connected to arduino pin 11

int maximumRange = 200;        // Maximum range needed, in cm
int minimumRange = 0;          // Minimum range needed, in cm
float duration, distance;      // Duration is used to calculate distance

// Delays given in us
int delayRadarTrig1 = 10;      // Delay after first trig LOW
int delayRadarTrig2 = 10;      // Delay after second trig HIGH
int delayRadarWait = 10000;    // Delay after each radar measurement
unsigned long timeout = 12000; // Time to wait for the echo signal in us

// Parameters associated with the stepper motor
int delayMotor = 1000;                      // Delay after each motor increment
int StepsPerRevolution = 512;               // Number of steps for one full turn
float ScanAngle = 90;                       // Scan angle width
float dphi = 360/float(StepsPerRevolution); // Scan discretization
int Nstep = round(ScanAngle/dphi);          // Number of steps for one scan

void setup() {
  Serial.begin(9600);            // Initialize serial port for communication
  pinMode(trigPin, OUTPUT);      // Define sensor trigger pin as output
  pinMode(echoPin, INPUT);       // Define sensor echo pin as input
  pinMode(powerPin, OUTPUT);     // Define power pin as output
  digitalWrite(powerPin, HIGH);  // Set power pin HIGH (+5V)
  
  // Initialize the motor pins as output
  pinMode(motorPin1, OUTPUT);
  pinMode(motorPin2, OUTPUT);
  pinMode(motorPin3, OUTPUT);
  pinMode(motorPin4, OUTPUT);
  // Make sure the motor is not running
  digitalWrite(motorPin1, LOW);
  digitalWrite(motorPin2, LOW);
  digitalWrite(motorPin3, LOW);
  digitalWrite(motorPin4, LOW);
}

void StepForward() {
  digitalWrite(motorPin1, HIGH);
  digitalWrite(motorPin2, LOW);
  digitalWrite(motorPin3, LOW);
  digitalWrite(motorPin4, LOW);
  delayMicroseconds(delayMotor);
  digitalWrite(motorPin1, HIGH);
  digitalWrite(motorPin2, HIGH);
  digitalWrite(motorPin3, LOW);
  digitalWrite(motorPin4, LOW);
  delayMicroseconds(delayMotor);
  digitalWrite(motorPin1, LOW);
  digitalWrite(motorPin2, HIGH);
  digitalWrite(motorPin3, LOW);
  digitalWrite(motorPin4, LOW);
  delayMicroseconds(delayMotor);
  digitalWrite(motorPin1, LOW);
  digitalWrite(motorPin2, HIGH);
  digitalWrite(motorPin3, HIGH);
  digitalWrite(motorPin4, LOW);
  delayMicroseconds(delayMotor);
  digitalWrite(motorPin1, LOW);
  digitalWrite(motorPin2, LOW);
  digitalWrite(motorPin3, HIGH);
  digitalWrite(motorPin4, LOW);
  delayMicroseconds(delayMotor);
  digitalWrite(motorPin1, LOW);
  digitalWrite(motorPin2, LOW);
  digitalWrite(motorPin3, HIGH);
  digitalWrite(motorPin4, HIGH);
  delayMicroseconds(delayMotor);
  digitalWrite(motorPin1, LOW);
  digitalWrite(motorPin2, LOW);
  digitalWrite(motorPin3, LOW);
  digitalWrite(motorPin4, HIGH);
  delayMicroseconds(delayMotor);
  digitalWrite(motorPin1, HIGH);
  digitalWrite(motorPin2, LOW);
  digitalWrite(motorPin3, LOW);
  digitalWrite(motorPin4, HIGH);
  delayMicroseconds(delayMotor);
}

void StepBackward() {
  digitalWrite(motorPin1, LOW);
  digitalWrite(motorPin2, LOW);
  digitalWrite(motorPin3, HIGH);
  digitalWrite(motorPin4, LOW);
  delayMicroseconds(delayMotor);
  digitalWrite(motorPin1, LOW);
  digitalWrite(motorPin2, HIGH);
  digitalWrite(motorPin3, HIGH);
  digitalWrite(motorPin4, LOW);
  delayMicroseconds(delayMotor);
  digitalWrite(motorPin1, LOW);
  digitalWrite(motorPin2, HIGH);
  digitalWrite(motorPin3, LOW);
  digitalWrite(motorPin4, LOW);
  delayMicroseconds(delayMotor);
  digitalWrite(motorPin1, HIGH);
  digitalWrite(motorPin2, HIGH);
  digitalWrite(motorPin3, LOW);
  digitalWrite(motorPin4, LOW);
  delayMicroseconds(delayMotor);
  digitalWrite(motorPin1, HIGH);
  digitalWrite(motorPin2, LOW);
  digitalWrite(motorPin3, LOW);
  digitalWrite(motorPin4, LOW);
  delayMicroseconds(delayMotor);
  digitalWrite(motorPin1, HIGH);
  digitalWrite(motorPin2, LOW);
  digitalWrite(motorPin3, LOW);
  digitalWrite(motorPin4, HIGH);
  delayMicroseconds(delayMotor);
  digitalWrite(motorPin1, LOW);
  digitalWrite(motorPin2, LOW);
  digitalWrite(motorPin3, LOW);
  digitalWrite(motorPin4, HIGH);
  delayMicroseconds(delayMotor);
  digitalWrite(motorPin1, LOW);
  digitalWrite(motorPin2, LOW);
  digitalWrite(motorPin3, HIGH);
  digitalWrite(motorPin4, HIGH);
  delayMicroseconds(delayMotor);
}

void Stop() {
  digitalWrite(motorPin1, LOW);
  digitalWrite(motorPin2, LOW);
  digitalWrite(motorPin3, LOW);
  digitalWrite(motorPin4, LOW);
  delayMicroseconds(delayMotor);
}

void Measure(int step) {
  unsigned long t0, t1;
  float phi;
  
  digitalWrite(trigPin, LOW);          // Initialize trigger 
  delayMicroseconds(delayRadarTrig1);  // Wait
  digitalWrite(trigPin, HIGH);         // Set trigger
  delayMicroseconds(delayRadarTrig2);  // Wait
  digitalWrite(trigPin, LOW);          // Restore trigger, starts the ultrasound

  while(digitalRead(echoPin) != HIGH); // Wait for the pin to go HIGH
  t0 = micros();                       // Then start the clock
  duration = 0;                        // Set initial duration to zero
  while((duration<timeout) and (digitalRead(echoPin) == HIGH)) // Wait for the pin to go LOW until the timeout
  {
    t1 = micros();                     // Get the time
    duration = t1 - t0;                // Update duration
  }

  // Compute and output angle
  phi = step*dphi - ScanAngle/2;
  Serial.print(phi);
  Serial.print(",");
  
  // Calculate the distance (in cm) based on the speed of sound.
  distance = duration/58.3;
  if (distance >= maximumRange || distance <= minimumRange){
    // If out of range, send a negative number to the serial port
    Serial.println("-1");
  }
  else {
    // Send the distance to the computer using the serial port
    Serial.println(distance);
  }
  delayMicroseconds(delayRadarWait);
}

void loop() {
  for (int n=0; n<Nstep; n++) {
    StepForward();
    Measure(n);
  }
  Stop();
  for (int n=0; n<Nstep; n++) {
    StepBackward();
    Measure(Nstep - n - 1);
  }
  Stop();
}