/*
  Blink

  Turns an LED on for one second, then off for one second, repeatedly.

  Most Arduinos have an on-board LED you can control. On the UNO, MEGA and ZERO
  it is attached to digital pin 13, on MKR1000 on pin 6. LED_BUILTIN is set to
  the correct LED pin independent of which board is used.
  If you want to know what pin the on-board LED is connected to on your Arduino
  model, check the Technical Specs of your board at:
  https://www.arduino.cc/en/Main/Products

  modified 8 May 2014
  by Scott Fitzgerald
  modified 2 Sep 2016
  by Arturo Guadalupi
  modified 8 Sep 2016
  by Colby Newman

  This example code is in the public domain.

  http://www.arduino.cc/en/Tutorial/Blink
*/

unsigned long int last_heartbeat_millis = 0;
int last_value = 0;
unsigned long int last_heartbeat_ms = 0;


int distance() {
  
  
  digitalWrite(2, HIGH);
  delayMicroseconds(10);
  digitalWrite(2, LOW);
  int Zeit = pulseIn(3, HIGH, 20000);
  int EntfernungL = (Zeit / 2) * 0.03432;
  delay(100);                       // wait for a second


  digitalWrite(4, HIGH);
  delayMicroseconds(10);
  digitalWrite(4, LOW);
  Zeit = pulseIn(5, HIGH, 20000);
  int EntfernungR = (Zeit / 2) * 0.03432;
//return EntfernungR;
  if (EntfernungL < EntfernungR) return EntfernungL;
  else return EntfernungR;
  }

// the setup function runs once when you press reset or power the board
void setup() {
  pinMode(13, OUTPUT);
  digitalWrite(13, LOW);  //Stop Platform
  pinMode(2, OUTPUT);     //Pins for ultrasonic
  pinMode(3, INPUT);
  pinMode(4, OUTPUT);
  pinMode(5, INPUT);
  pinMode(6, INPUT_PULLUP);    //HEARTBEAT

    Serial.begin(115200);
}





void update_hearbeat()
{
  int value = digitalRead(6);
  if(value != last_value)
  {
    last_heartbeat_millis = millis();
    last_value = value;
  }
Serial.println(value);
}



// the loop function runs over and over again forever
void loop() {
//  digitalWrite(13, HIGH);   // turn the LED on (HIGH is the voltage level)
// delay(100);                       // wait for a second
//  digitalWrite(13, LOW);    // turn the LED off by making the voltage LOW
//  delay(100);                       // wait for a second
  update_hearbeat();
  delay(100);                       // wait for a second
  
  unsigned long  current_millis = millis();
  if(((current_millis - last_heartbeat_millis) > (unsigned long) 1000) ||  (distance() < 15))
  {
    //STOP
    Serial.print(current_millis - last_heartbeat_millis);
    digitalWrite(13, LOW); //STOP
    Serial.println("Stop");
    
  }
  else
  {
    Serial.print(current_millis - last_heartbeat_millis);
    digitalWrite(13, HIGH);    // If Distance enough activate Motor-drivers
    Serial.println("RUN");
  }


  Serial.println(distance());

  
}