Unterschiede

Hier werden die Unterschiede zwischen zwei Versionen angezeigt.

--- hello_mbot_kalibrierung [2025/02/19 09:57] – torsten.roehl
+++ hello_mbot_kalibrierung [2025/02/19 19:55] (aktuell) – [Bedienung] torsten.roehl
@@ Zeile 97: / Zeile 97: @@
 ==== Quellcode (engl. Sourcecode) ====
-FIXME  Code noch nicht fertig!
 <Code c linenums:1 | Listing 1:MinimalesProgramm.ino>
+#include "MeMCore.h"
-    #include "MeMCore.h"
+// Hardware
+MeBuzzer buzzer;
-    MeRGBLed led(0, 2);   // must be fixed!
+MeLineFollower lineFinder(PORT_2);
-    MeDCMotor motor1(M1);
+MeRGBLed led(0, 2);   // must be fixed!
-    MeDCMotor motor2(M2);
+MeDCMotor motor1(M1);
-    int PIN_BUTTON = 7;   // must be fixed!
+MeDCMotor motor2(M2);
-    int threshold  = 500; // Analoger Schwellenwert!
+int PIN_BUTTON = 7;   // must be fixed!
-    int buttonCount;
+int threshold  = 500; // Analoger Schwellenwert!
-    unsigned long time90degree;
+int buttonCount;
+// Allgemein
-    enum State {
+int speed = 100; // Motor speed
-      STATE_OFF,
+unsigned long time90degree;
-      STATE_CALIBRATION,
-      STATE_TEST
+// turn90Degree...
-    };
+bool t90d_isTurning = false;
-    State state = STATE_OFF;
+unsigned long t90d_turnStartTime = 0;
+// actionCalibration...
-    void setup() {
+unsigned long ac_turnStartTimeLeft  = 0;
-      led.setpin(13);
+unsigned long ac_turnTimeLeft = 0;
-      pinMode(PIN_BUTTON, INPUT);
+unsigned long ac_turnStartTimeRight = 0;
-      buttonCount = 0;
+unsigned long ac_turnTimeRight = 0;
-    }
+bool ac_isTurning = false;
+bool ac_isLeftTurning = false;
-    void loop() {
+bool ac_isRightTurning = false;
-      // step: command
+bool ac_calibrationDone = false;
-      int cmd = read();
+//actionTest...
-      // step: state
+bool at_doneLeft = false;
-      state  = decode(cmd);
+bool at_doneRight = false;
-      // step: action
+bool at_testComplete = false;
-      switch (state) {
-        case STATE_CALIBRATION:
+enum State {
-          actionCalibration();
+  STATE_OFF,
-          break;
+  STATE_CALIBRATION,
-        case STATE_TEST:
+  STATE_TEST
-          actionTest();
+};
-          break;
+State state = STATE_OFF;
-        case STATE_OFF:
-          actionOff();
+void setup() {
-          break;
+  led.setpin(13);
-      }
+  pinMode(PIN_BUTTON, INPUT);
-    }
+  buttonCount = 0;
+}
-    /*
+void loop() {
-     *   Funktionen
+  // step: command
-     */
+  int cmd = read();
-    bool turn90Degree(bool left){
+  // step: state
-      // TODO (nicht-blockierende methode)
+  state  = decode(cmd);
+  // step: action
+  switch (state) {
+    case STATE_CALIBRATION:
+      actionCalibration();
+      break;
+    case STATE_TEST:
+      actionTest();
+      break;
+    case STATE_OFF:
+      actionOff();
+      break;
+  }
+}
+/*
+     Funktionen
+*/
+bool turn90Degree(bool left) {
+  // step: ...start turning
+  if (!t90d_isTurning) {
+    motor1.stop();
+    motor2.stop();
+    t90d_isTurning     = true;
+    int dir = left ? 1 : -1;
+    motor1.run(dir * speed);
+    motor2.run(dir * speed);
+    t90d_turnStartTime = millis();
+    return false;
+  }
+  // step: ...in progress
+  if (millis() - t90d_turnStartTime < time90degree)
+    return false;
+  // step: ... finished
+  motor1.stop();
+  motor2.stop();
+  t90d_isTurning = false;
+  return true;
+}
+bool isButtonPressed() {
+  static bool buttonPressed = false;
+  int value = analogRead(PIN_BUTTON);
+  if (value < threshold) {
+    if (!buttonPressed) {
+      buttonPressed = true;
       return true;
     }
+  } else {
-    bool isButtonPressed() {
+    buttonPressed = false; // Button wurde losgelassen
-      static bool buttonPressed = false;
+  }
-      int value = analogRead(PIN_BUTTON);
+  return false;
-      if (value < threshold) {
+}
-        if (!buttonPressed) {
-          buttonPressed = true;
+int read() {
-          return true;
+  if ( isButtonPressed() )
-        }
+    buttonCount += 1;
-      } else {
-        buttonPressed = false; // Button wurde losgelassen
+  if (buttonCount > 2)
-      }
+    buttonCount = 0;
-      return false;
+  return buttonCount;
-    }
+}
-    int read() {
-      if ( isButtonPressed() )
+State decode(int cmd) {
-        buttonCount += 1;
+  switch (cmd) {
+    case 1: return STATE_CALIBRATION;
-      if (buttonCount > 2)
+    case 2: return STATE_TEST;
-        buttonCount = 0;
+  }
+  return STATE_OFF;
-      return buttonCount;
+}
-    }
+void actionCalibration() {
-    State decode(int cmd) {
+  if (ac_calibrationDone)
-      switch (cmd) {
+    return;
-        case 1: return STATE_CALIBRATION;
-        case 2: return STATE_TEST;
+  led.setColorAt(1, 255, 0, 0);
-      }
+  led.setColorAt(0, 255, 0, 0);
-      return STATE_OFF;
-    }
-    void actionCalibration() {
-      led.setColorAt(1, 255, 0, 0);
-      led.setColorAt(0, 255, 0, 0);
-      led.show();
-      // TODO calibrate 90degree here!
-    }
-  void actionTest() {
-  led.setColorAt(1, 0, 255, 0);
-  led.setColorAt(0, 0, 255, 0);
   led.show();
+  int sensorState = lineFinder.readSensors();
-  static bool doneLeft = false;
+  // step - start turning left
-  static bool doneRight = false;
+  if (!ac_isTurning) {
-  //
+    motor1.stop();
-  if (!doneLeft) {
+    motor2.stop();
-    doneLeft = turn90Degree(true);
+    motor1.run(speed);
-  }
+    motor2.run(speed);
-  //
+    ac_turnStartTimeLeft = millis();
-  else if (doneLeft && !doneRight) {
+    ac_isTurning = true;
-    doneRight = turn90Degree(false);
+    ac_isRightTurning = true;
+    do {
+      sensorState = lineFinder.readSensors();
+    } while ( sensorState != S1_OUT_S2_OUT);
   }
-  // Wenn beide Drehungen abgeschlossen sind, gehe in den "OFF"-Zustand
-  else if (doneLeft && doneRight) {
+  // step - end turning left ... start turning right
-    state = STATE_OFF;
+  if (ac_isRightTurning && sensorState == S1_IN_S2_IN) {
+    motor1.stop();
+    motor2.stop();
+    ac_turnTimeLeft = millis() - ac_turnStartTimeLeft;
+    motor1.run(-speed);
+    motor2.run(-speed);
+    ac_turnStartTimeRight = millis();
+    do {
+      sensorState = lineFinder.readSensors();
+    } while ( sensorState != S1_OUT_S2_OUT);
+    ac_isRightTurning = false;
+    ac_isLeftTurning = true;
   }
+  // step - end turing right
+  if (ac_isLeftTurning && sensorState == S1_IN_S2_IN) {
+    motor1.stop();
+    motor2.stop();
+    ac_turnTimeRight = millis() - ac_turnStartTimeRight;
+    time90degree = (ac_turnTimeLeft + ac_turnTimeRight) / 2.0;
+    ac_calibrationDone = true;
+    led.setColorAt(1, 0, 0, 0);
+    led.setColorAt(0, 0, 0, 0);
+    led.show();
+    buzzer.tone(1200, 600);
+  }
+}
+void actionTest() {
+  if (!at_doneLeft) { // Links-Drehung
+    at_doneLeft = turn90Degree(true);
+    led.setColorAt(1, 0, 255, 0);
+    led.setColorAt(0, 0, 255, 0);
+    led.show();
+    return;
+  }
+  if (!at_doneRight) { // Rechts-Drehung
+    at_doneRight = turn90Degree(false);
+    return;
+  }
+  if (!at_testComplete) { // fertig
+    motor1.stop();
+    motor2.stop();
+    led.setColorAt(1, 0, 0, 0);
+    led.setColorAt(0, 0, 0, 0);
+    led.show();
+    buzzer.tone(1200, 600);
+    at_testComplete = true;
+    return;
+  }
+}
+void actionOff() {
+  led.setColorAt(1, 0, 0, 0);
+  led.setColorAt(0, 0, 0, 0);
+  led.show();
+  // reset
+  motor1.stop();
+  motor2.stop();
+  // reset state calibration
+  ac_isTurning = false;
+  ac_isLeftTurning = false;
+  ac_isRightTurning = false;
+  ac_calibrationDone = false;
+  // reset state test
+  at_doneLeft = false;
+  at_doneRight = false;
+  at_testComplete = false;
 }
-    void actionOff() {
-      led.setColorAt(1, 0, 0, 0);
-      led.setColorAt(0, 0, 0, 0);
-      led.show();
-      motor1.stop();
-      motor2.stop();
-    }
 </Code>
 ==== Bedienung ====
@@ Zeile 240: / Zeile 345: @@
 ⚡ **Schritt 3:**
-▶ **Nach dem Testlauf** kehrt der mBot in den **Ruhezustand** zurück.
+▶ **Nach dem Testlauf** kehrt der mBot durch Drückens des Tasters in den **Ruhezustand** zurück.
    * Der Vorgang kann **von vorne** gestartet werden.