#include <Stepper.h>
#include <NewPing.h>

#define STEPS 100 // number of steps per revolution



const int pingPin = 13; 
int inPin = 12;
int safeZone = 5;


Stepper motor(STEPS, 8, 9, 10, 11);
//NewPing sodar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);
int ping_delay = 50;
long last_ping_time = 0;
long duration; // duration of return pulse

boolean new_reading = false;
const int max_write_delay = 50;
long last_write_time = 0;



boolean stopMotor = false;
int rpm = 45; // default RPM

void setup() {
  motor.setSpeed(rpm);
  // initialize serial communication
  Serial.begin(9600);
}

void loop()
{
  long duration, cm;
  pinMode(pingPin, OUTPUT);
  digitalWrite(pingPin, LOW);
  delayMicroseconds(2);
  digitalWrite(pingPin, HIGH);
  delayMicroseconds(5);
  digitalWrite(pingPin, LOW);

  pinMode(inPin, INPUT);
  duration = pulseIn(inPin, HIGH);
  cm = microsecondsToCentimeters(duration);
  
  if(millis() - last_ping_time >= ping_delay) {
    last_ping_time = millis();
  
    // trigger the pulse and interrupt on response
  //  sodar.ping_timer(checkPing);
  }
  
  if(new_reading) {
    new_reading = false;
    last_write_time = millis();
    
    // convert the time into a distance
   // cm = microsecondsToCentimeters(duration);
   // cm = random(1, 300);
    
    writeData(cm, motor.getAngle());
  } else if(millis() - last_write_time >= max_write_delay) {
    last_write_time = millis();
    
   // cm = 0;
    writeData(cm, motor.getAngle());
  }
  
  if(!stopMotor) {
    motor.step(1);
  }
}

void writeData(float cm, float angle)
{
  
 Serial.print(cm);
Serial.print(" cm @ ");
  
  
  Serial.print(angle);
  Serial.println(" degrees");
}



long microsecondsToCentimeters(long microseconds)
{
  return microseconds / 29 / 2;
}