Различия

Здесь показаны различия между двумя версиями данной страницы.

--- 10i_2025_algo [2025/04/22 15:46]
super_admin
+++ 10i_2025_algo [2025/10/07 14:22] (текущий)
super_admin [РОБОТ МФТИ]
@@ Строка 485: / Строка 485: @@
 if __name__ == '__main__':
     run_tasks()
+</code>
+<code>
+const int LM_s1 = 10; // левый мотор вход 1
+const int LM_s2 = 9; // левый мотор вход 2
+const int RM_s1 = 5; // правый мотор вход 1
+const int RM_s2 = 6; // правый мотор вход 2
+int led_pin = 13;
+// Укажем пины для подключения УЗ датчика
+int trigPin = 12;
+int echoPin = 11;
+int s_for_obj = 0;
+// текущее состояние системы
+int state = 0; // начальное состояние
+void setup(){
+ Serial.begin(9600);
+ //правый
+    pinMode(RM_s1, OUTPUT);
+    pinMode(RM_s2, OUTPUT);
+ //левый
+    pinMode(LM_s1, OUTPUT);
+    pinMode(LM_s2, OUTPUT);
+// УЗ
+    pinMode(trigPin, OUTPUT);
+    pinMode(echoPin, INPUT);
+// светодиод
+    pinMode(led_pin, OUTPUT);
+}
+int dist() // функция для вычисления расстояния
+{
+  int t,s; // локальные переменные время, расстояние
+  // установить высокий уровень на пине Trig
+  digitalWrite(trigPin, 1);
+  // подождать 10 мс
+  delay(10/16);
+  digitalWrite(trigPin, 0);
+  // узнаем время
+  t = pulseIn(echoPin, 1);
+  // вычисляем расстояние
+  s = t * 0.034/2;
+  return (s);
+}
+void loop() {
+    s_for_obj = dist();
+    //Serial.println(s_for_obj );// вывод измеренного расстояния в окно сообщений
+    //delay(20/16);
+Serial.println(state);
+switch (state) {
+  case 0: // начальное состояние: вперёд до первого препятствия
+  case 2: // после первого поворота: вперёд до второго препятствия
+    if (s_for_obj == 0 || s_for_obj > 50){
+         moveForward();
+    } else {
+         stopMotors();
+         state ++;
+    }
+    break;
+  case 1: // перед первым препятствием: левый поворот
+  case 3: // перед вторым препятствием: левый поворот
+    turnLeft();
+    delay(2970/16);
+    state ++;
+    stopMotors();
+    break;
+  case 4: // после второго поворота: прямолинейный путь до остановки
+    moveForward();
+    delay(8000/16); // время прямолинейного движения до остановки
+    state ++;
+    stopMotors();
+    break;
+  case 5: // после остановки на площадке: включить светодиод на 4 секунды
+    // ...
+    break;
+  default:
+     stopMotors();
+  break;
+}
+}
+void moveForward() {
+     //Serial.println("up");
+    digitalWrite(LM_s1, 1);
+    digitalWrite(LM_s2, 0);
+    digitalWrite(RM_s1, 1);
+    digitalWrite(RM_s2, 0);
+ delay(20);
+}
+void moveBackward() {
+    //Serial.println("down");
+    digitalWrite(LM_s1, 0);
+    digitalWrite(LM_s2, 1);
+    digitalWrite(RM_s1, 0);
+    digitalWrite(RM_s2, 1);
+ delay(20);
+}
+void turnLeft() {
+    digitalWrite(LM_s1, LOW);
+    digitalWrite(LM_s2, HIGH);
+    digitalWrite(RM_s1, HIGH);
+   digitalWrite(RM_s2, LOW);
+ delay(20);
+}
+void turnRight() {
+    digitalWrite(LM_s1, HIGH);
+    digitalWrite(LM_s2, LOW);
+    digitalWrite(RM_s1, LOW);
+   digitalWrite(RM_s2, HIGH);
+ delay(20);
+}
+void stopMotors() {
+    digitalWrite(LM_s1, LOW);
+    digitalWrite(LM_s2, LOW);
+    digitalWrite(RM_s1, LOW);
+   digitalWrite(RM_s2, LOW);
+ //delay(20);
+}
 </code>

INTEPRA WIKI

Инструменты пользователя

Инструменты сайта

Различия

Инструменты страницы