#include <Arduino.h>
/*
Система управления мобильной загрузочной тележкой.
Тележка перемещает загрузочную трубу вдоль направляющих
к зоне загрузки мельницы с центральной разгрузкой.
Wokwi-модель:
- кнопки FORWARD, BACK, STOP, E-STOP;
- передний и задний HC-SR04;
- концевики HOME и END;
- светодиоды имитируют двигатель, аварию и подачу материала.
*/
const int BTN_FORWARD = 32;
const int BTN_BACK = 33;
const int BTN_STOP = 25;
const int BTN_ESTOP = 26;
const int LIMIT_HOME = 27;
const int LIMIT_END = 14;
const int TRIG_FRONT = 5;
const int ECHO_FRONT = 18;
const int TRIG_BACK = 19;
const int ECHO_BACK = 21;
const int LED_FORWARD = 12;
const int LED_BACK = 13;
const int LED_ERROR = 15;
const int LED_FEED = 2;
const int DRIVER_FWD = 22;
const int DRIVER_REV = 23;
const float OBSTACLE_DISTANCE_CM = 25.0;
const unsigned long FEED_TIME_MS = 3000;
enum CartState {
WAITING,
MOVING_FORWARD,
MOVING_BACK,
FEEDING,
STOPPED,
ERROR_STATE
};
CartState state = WAITING;
unsigned long feedStartTime = 0;
bool pressed(int pin) {
return digitalRead(pin) == LOW;
}
float readDistanceCm(int trigPin, int echoPin) {
digitalWrite(trigPin, LOW);
delayMicroseconds(2);
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
unsigned long duration = pulseIn(echoPin, HIGH, 30000);
if (duration == 0) return 400.0;
return duration / 58.0;
}
void motorStop() {
digitalWrite(LED_FORWARD, LOW);
digitalWrite(LED_BACK, LOW);
digitalWrite(DRIVER_FWD, LOW);
digitalWrite(DRIVER_REV, LOW);
}
void motorForward() {
digitalWrite(LED_FORWARD, HIGH);
digitalWrite(LED_BACK, LOW);
digitalWrite(DRIVER_FWD, HIGH);
digitalWrite(DRIVER_REV, LOW);
}
void motorBack() {
digitalWrite(LED_FORWARD, LOW);
digitalWrite(LED_BACK, HIGH);
digitalWrite(DRIVER_FWD, LOW);
digitalWrite(DRIVER_REV, HIGH);
}
void setup() {
Serial.begin(115200);
pinMode(BTN_FORWARD, INPUT_PULLUP);
pinMode(BTN_BACK, INPUT_PULLUP);
pinMode(BTN_STOP, INPUT_PULLUP);
pinMode(BTN_ESTOP, INPUT_PULLUP);
pinMode(LIMIT_HOME, INPUT_PULLUP);
pinMode(LIMIT_END, INPUT_PULLUP);
pinMode(TRIG_FRONT, OUTPUT);
pinMode(ECHO_FRONT, INPUT);
pinMode(TRIG_BACK, OUTPUT);
pinMode(ECHO_BACK, INPUT);
pinMode(LED_FORWARD, OUTPUT);
pinMode(LED_BACK, OUTPUT);
pinMode(LED_ERROR, OUTPUT);
pinMode(LED_FEED, OUTPUT);
pinMode(DRIVER_FWD, OUTPUT);
pinMode(DRIVER_REV, OUTPUT);
motorStop();
digitalWrite(LED_ERROR, LOW);
digitalWrite(LED_FEED, LOW);
Serial.println("Система управления загрузочной тележкой запущена.");
}
void loop() {
float frontDistance = readDistanceCm(TRIG_FRONT, ECHO_FRONT);
float backDistance = readDistanceCm(TRIG_BACK, ECHO_BACK);
bool obstacleFront = frontDistance < OBSTACLE_DISTANCE_CM;
bool obstacleBack = backDistance < OBSTACLE_DISTANCE_CM;
bool atHome = pressed(LIMIT_HOME);
bool atEnd = pressed(LIMIT_END);
if (pressed(BTN_ESTOP)) {
motorStop();
digitalWrite(LED_FEED, LOW);
digitalWrite(LED_ERROR, HIGH);
state = ERROR_STATE;
}
if (state == ERROR_STATE) {
if (!pressed(BTN_ESTOP) && pressed(BTN_STOP)) {
digitalWrite(LED_ERROR, LOW);
state = STOPPED;
Serial.println("Авария сброшена.");
delay(300);
} else {
delay(100);
return;
}
}
if (pressed(BTN_STOP)) {
motorStop();
digitalWrite(LED_FEED, LOW);
state = STOPPED;
}
switch (state) {
case WAITING:
case STOPPED:
motorStop();
digitalWrite(LED_FEED, LOW);
if (pressed(BTN_FORWARD)) {
if (atEnd) {
Serial.println("Тележка уже находится у мельницы.");
} else if (obstacleFront) {
Serial.println("Движение вперёд запрещено: препятствие спереди.");
digitalWrite(LED_ERROR, HIGH);
state = ERROR_STATE;
} else {
Serial.println("Движение вперёд к мельнице.");
state = MOVING_FORWARD;
}
delay(300);
}
if (pressed(BTN_BACK)) {
if (atHome) {
Serial.println("Тележка уже находится в исходном положении.");
} else if (obstacleBack) {
Serial.println("Движение назад запрещено: препятствие сзади.");
digitalWrite(LED_ERROR, HIGH);
state = ERROR_STATE;
} else {
Serial.println("Движение назад.");
state = MOVING_BACK;
}
delay(300);
}
break;
case MOVING_FORWARD:
if (obstacleFront) {
motorStop();
digitalWrite(LED_ERROR, HIGH);
state = ERROR_STATE;
Serial.println("Остановка: препятствие спереди.");
} else if (atEnd) {
motorStop();
state = FEEDING;
feedStartTime = millis();
digitalWrite(LED_FEED, HIGH);
Serial.println("Достигнута зона загрузки. Имитация подачи материала.");
} else {
motorForward();
}
break;
case MOVING_BACK:
if (obstacleBack) {
motorStop();
digitalWrite(LED_ERROR, HIGH);
state = ERROR_STATE;
Serial.println("Остановка: препятствие сзади.");
} else if (atHome) {
motorStop();
state = STOPPED;
Serial.println("Тележка вернулась в исходное положение.");
} else {
motorBack();
}
break;
case FEEDING:
motorStop();
if (millis() - feedStartTime >= FEED_TIME_MS) {
digitalWrite(LED_FEED, LOW);
state = STOPPED;
Serial.println("Подача материала завершена.");
}
break;
default:
motorStop();
state = STOPPED;
break;
}
Serial.print("State=");
Serial.print(state);
Serial.print(" Front=");
Serial.print(frontDistance);
Serial.print(" cm Back=");
Serial.print(backDistance);
Serial.print(" cm HOME=");
Serial.print(atHome);
Serial.print(" END=");
Serial.println(atEnd);
delay(150);
}Loading
esp32-devkit-c-v4
esp32-devkit-c-v4
Тележка с загрузочной трубой: движение по направляющим вперёд/назад.
DRIVER FWD/REV условно заменяют реальный драйвер двигателя.