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

#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

// Delays given in us
int delayRadarTrig1 = 10;       // Delay after first trig LOW
int delayRadarTrig2 = 10;       // Delay after second trig HIGH
unsigned long timegate = 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;                // Variables used for timing
  unsigned long target_delay;          // Time delay to target
  unsigned long duration;              // Time we have been waiting for echo
  float distance;                      // Duration is used to calculate distance
  float phi;                           // Angle in which we are measuring
  
  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
  target_delay = 0;                    // Set initial target delay
  while(duration < timegate)           // Wait until the timegate
  {
    t1 = micros();                     // Get the time
    duration = t1 - t0;                // Update duration
    if ((digitalRead(echoPin) == LOW) and (target_delay == 0))
      target_delay = 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 = target_delay/58.3;
  if ((distance >= maximumRange) || (distance <= minimumRange) || (target_delay == 0))
  {
    // If out of range, make distance -1
    distance = -1;
  }
  // Send the distance to the computer using the serial port
  Serial.println(distance);
}

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