91 lines
2.1 KiB
C++
91 lines
2.1 KiB
C++
/*
|
|
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());
|
|
|
|
|
|
}
|