Ich habe hier mal eine Lösung mit einem Servo und einer State Machine erstellt, die momentan auf Taster und Leds verzichtet (lassen sich einfach "nachrüsten"), und die von mir oben genannten Aspekte berücksichtigen sollte ...
/*
Steuerung eines Servos mit einer State Machine
Programmiert von ec2021
Stand: 25.02.2023, 15:40 Uhr
INIT:
Initialisierung des Servos, der die Winkelstellung OpenAngle einnimmt.
Danach Wechsel zu WAIT
WAIT:
Regelmässiges Messen der Entfernung zu einem Objekt
Ist die Entfernung für "AcceptTime" Sekunden kürzer oder gleich der "CloseDistance" cm
und ist der vorherige Zustand OPENING so wird der Zustand CLOSING gestartet.
War der vorherige Zustand CLOSING oder INIT, bleibt es bei WAIT.
Ist die Enfernung größer als "OpenDistance" cm und der vorherige Zustand INIT oder CLOSING,
so wird der Zustand OPENING gestartet.
In allen anderen Fällen, bleibt der Zustand auf WAIT
OPENING:
Fährt den Servo in Stellung "OpenAngle" mit Einzelschritt alle "stepTimeOpening" ms
Beim Erreichen des "OpenAngle" wechselt der Zustand zu WAIT.
CLOSING:
Fährt den Servo in Stellung "CloseAngle" mit Einzelschritt alle "stepTimeClosing" ms
Beim Erreichen des "CloseAngle" wechselt der Zustand zu WAIT.
*/
#include <ESP32Servo.h>
static const int servoPin = 14; //Anschlusspin des Servos
static int trigPin = 23; // Triggerpin für den Ultraschall-E-Messer
static int echoPin = 22; // Echopin für den Ultraschall-E-Messer
static unsigned long stepTimeClosing = 20; // Einzelschrittzeit in ms beim CLOSING
static unsigned long stepTimeOpening = 20; // Einzelschrittzeit in ms beim OPENING
static unsigned long AcceptTime = 10000; // 10 Sekunden
static int OpenAngle = 90; // Winkelstellung nach OPENING
static int ClosedAngle = 180; // Winkelstellung nach CLOSING
static int CloseDistance = 5; // Maximalentfernung, die zu CLOSING führt,
// wenn sie für AcceptTime ms besteht
static int OpenDistance = 15; // Minimalenfernung, die sofort zum Öffnen führt
Servo servo1;
int ActualAngle = 90; // Aktuelle Winkelvorgabe des Servos
unsigned long CloseDistanceTime = 0; // Systemzeit in ms, zu der zuletzt eine
// Entfernung größer als CloseDistance gemessen wurde
int dist = 0; // Letzter gemessener Entfernungswert
void setup() {
Serial.begin(115200);
Serial.println("Start");
servo1.attach(servoPin);
pinMode(trigPin, OUTPUT);
pinMode(echoPin, INPUT);
}
void StateMachine(){
enum State {INIT,OPENING,CLOSING,WAIT};
static State state = INIT;
static State prevState = INIT;
static uint32_t previousMillis = 0;
switch(state) {
case INIT : {
Serial.println("INIT");
servo1.write(ActualAngle);
state = WAIT;
prevState = INIT;
previousMillis = millis();
Serial.println("WAIT");
break;
}
case WAIT : {
if (millis() - previousMillis > 100){
dist = getDistance();
if (prevState == INIT || prevState == OPENING) {
if (dist > CloseDistance) CloseDistanceTime = millis();
if (millis()-CloseDistanceTime > AcceptTime){
state = CLOSING;
Serial.println("CLOSING");
}
}
if (prevState == CLOSING) {
if (dist > OpenDistance) {
state = OPENING;
Serial.println("OPENING");
}
}
previousMillis = millis();
};
break;
}
case OPENING : {
if (millis() - previousMillis > stepTimeOpening){
if (ActualAngle > OpenAngle) ActualAngle--;
previousMillis = millis();
}
if (ActualAngle == OpenAngle) {
prevState = state;
state = WAIT;
Serial.println("WAIT ");
previousMillis = millis();
}
break;
}
case CLOSING: {
if (millis() - previousMillis > stepTimeClosing){
if (ActualAngle < ClosedAngle) ActualAngle++;
previousMillis = millis();
}
if (ActualAngle == ClosedAngle) {
prevState = state;
state = WAIT;
Serial.println("WAIT");
previousMillis = millis();
}
break;
}
}
servo1.write(ActualAngle);
}
int getDistance() {
float duration_us, distance_cm;
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
duration_us = pulseIn(echoPin, HIGH);
distance_cm = 0.017 * duration_us;
return int(distance_cm);
}
void loop() {
StateMachine();
}
Der Sketch läuft bei Wokwi unter
https://wokwi.com/projects/357561697858544641
In dieser Version wird der Servo beim Start ohne zeitliche Steuerung in den OpenAngle kommandiert, so dass eine definierte Ausgangslage besteht. Steht der Roboter in der Station (die gemessene Entfernung ist für AcceptTime kleiner/gleich der CloseDistance, so wird das Tor nach AcceptTime geschlossen. In diesem Fall dauert das nach dem Einschalten 10 Sekunden. AcceptTime kann gerne auch kürzer sein, sollte aber so gewählt werden, dass ein verfrühtes und unnötiges Schließen vermieden wird.
Wenn die Startlage "Geschlossen" sein soll, kann das natürlich auch angepasst werden. Vermutlich ist es aber beim Einschalten der Anlage günstiger, wenn das Tor auf ist und nur schließt, wenn der Roboter auch in der Station steht.
Zwischen CloseDistance und OpenDistance habe ich noch einen Abstand gelassen, der auch bei kurzer AcceptTime ein hin- und her vermeiden sollte.
