/*
Arduino zur Kontrolle der Motortreiber (Sicherheitfunktionen)
Distance un Heartbeat
*/

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

  
}