DirtDevil

Reanimation eines Putzroboters
Plötzlich streikte unser Dirt-Devil Putzroboter... Auch ein neuer Akku brachte keine Lösung und damit die Anschaffung des Akkus nicht vergebens war, wollte ich versuchen, das Gerät mit einer neuen Steuerung zu versehen, denn die Funktionselemente wie Antrieb, Saugturbine,, Kontaktsensoren usw. waren noch ok. Google half weiter mit einer Aufstellung der Schaltpunkte für die internen Aktivatoren. Also war eine Reparatur mittels Arduino Mikrocontroller in den Bereich des Möglichen gerückt. Die technische Umsetzung wurde mittels eines programmierbaren Arduino-Mikrocontrollers gestaltet.

Im Rahmen der Neuprogrammierung des Putzroboters konnte ich eine Lösung für ein besonderes Problem in unserer Wohnung finden, denn das Gerät hing leider immer wieder rettungslos am Rund-Fuß eines Beistelltisches fest, weil dieser mit einer Schräge am Boden stand. Auf diese rund verlaufend Schräge fuhr der Roboter immer wieder auf, ohne dabei den Stoßkontakt auszulösen, die Antriebsräder drehten kraftlos weiter und der Roboter war blockiert, ohne sich zu befreien. Abhilfe war ganz einfach dadurch zu erzielen, dass die Steuerungs-Software analysiert, wie lange die letzte Kollision mit einem Gegenstnd her ist. Und wenn 30 Sekunden keine Kollision eintrat, dann hängt das Gerät wahrscheinlich und es wird eine befreiende Rückwärts-Fahrt vorgenommen. Durch diese Maßnahme war der Tischfuß absolut kein Problem mehr... Nicht berücksichtigt werden konnte bei der Neuprogrammierung die Treppen-Sensorik und die Ladesteuerung für den Akku. Das Thema 'Treppen' war dabei kein Problem, da unser Wohnbereich ebenerdig ist. Und das Laden des Akkus führte ich manuell mit einem externen Netzteil durch. Das ist der Dirt-Devil-Sketch:

// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
// Dirt Devil 'Libero' (baugleich 'Spider')-Steuerung V1.60 (nano, Old Bootloader)
// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
// o) Flexible Streckenführung durch zufällig lange Drehungen nach Kollisionen (auch zufällig links/rechts in längeren Richtungsgruppen)
// o) 2 abwechselnde Betriebsarten: a) geradlinige Strecke b) Kantenmodus mit leichter Kurvenführung (jeweils in Richtungsgruppen links/rechts)
// o) Interrupt-gesteuerte Kollisionserkennung über den internen Front-Sensor
// o) abwechselnde links/rechts-Weiterfahrt
// o) nach Kollissionen zufallsgesteuerte Befreiungsaktion
// o) KEINE Berücksichtigung der Treppensturz-Sensoren
// o) Blockade-Bewältigung mit gesonderter Befreiungsaktion, falls zu lange keine Kollision registriert wird
//    aber TIMEOUT-Abschaltung bei zwei aufeinanderfolgenden Blockaden ohne Zwischen-Kollision
// o) Überwachung der Akkuspannung, Stopp bei Unterspannung, ca. 5min akustisches Warnsignal, danach kompletter Shutdown
// o) Gesamt-Shutdown bei zu niedriger Akkuspannung nach abgeschlossener Signal-Meldedauer
// o) Möglichkeit zu programmseitigem Testmodus für gezieltes Austesten bestimmter Teil-Funktionen

// Offene Probleme:
// o) Akku-Ladesteuerung fehlt

int TEST          =   0;           // > 0 führt ausschließlich die jeweils gewünschte Testroutine aus
int RECHTS_VOR    =  10;           // Pin-Zuordnungen der Motoren
int LINKS_VOR     =   5;
int RECHTS_REV    =   9;
int LINKS_REV     =   6;
int ANLAUF        =   0;           // Zähler für Sanftanlauf
int BEEP          =   7;           // Buzzer mit 100 Ohm nach GND verschaltet
int TIMEOUT       =   0;           // Zähler für Anzahl Timeout-Situationen in Folge
int POWER         =   8;           // Power-Steuerungs-Pin zum NPN-Transistor (siehe Schaltbild dirtdevil.fzz)
long DAUER        =   0;           // Zufällige Dauer bei Befreiungsaktion
int COUNTER       = 100;           // Links-/Rechts-Zähler
int LI_RE         =   1;           // Links-/Rechts-Merker (1=rechts, 0=links)
int MOD_CNT       =   0;           // Modus-Zähler (0..50 = Normalmodus, 51..99 = Kantenmodus)
int KANTE         =   0;           // 0=normal, 1=KANTEnmodus
int SAUGER        =   4;           // Gebläse & Bürsten
const byte BUMPER =   2;           // verwendeter Interrupt-Port
long SPANNUNG     =   0;           // Akku-Spannung
int AnalogPin     =  A0;           // an A0 messen wir die Akkuspannung
long Umin         = 620;           // minimale Akkuspannung, ab dem der Robbie stoppt und das akustische Signal ertönt
unsigned long ZEIT;                // Zeitkontrolle gegen Hänger-Situationen
int AKTIV         =  1;            // Laufvariable setzen auf 'STARTEN'

// #define send ;                    // Schalter des 'Seriellen Monitors' zum Debuggen

// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
void setup() {                     // Controller vorbereiten
  pinMode(BEEP,      OUTPUT); 
  digitalWrite(BEEP,    LOW);
  pinMode(RECHTS_VOR,OUTPUT);
  pinMode(LINKS_VOR, OUTPUT);
  pinMode(RECHTS_REV,OUTPUT);
  pinMode(LINKS_REV, OUTPUT);
  pinMode(SAUGER,    OUTPUT);
  pinMode(POWER,     OUTPUT);
  digitalWrite(POWER,  HIGH);      // Versorgungs-Transistor aktivieren
  pinMode(BUMPER,     INPUT);      // Bumper-Kontakt
  randomSeed(analogRead(0));       // Zufalls-Routine geeignet initialisieren, um wirklich zufällige Werte bei 'random()' zu erhalten
  
  #ifdef send
    Serial.begin(9600);            // Bildschirmanzeige starten
    Serial.println("<<< START >>>");
  #endif
  STOP();
  attachInterrupt(digitalPinToInterrupt(BUMPER), isr, HIGH); // Interrupt-Routine 'isr' aktivieren für BUMPER-Verarbeitung
  SIGNAL_INFO();
}

// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
void STOP()                        // alle Motoren STOPPEN
{
  #ifdef send
    Serial.println("STOP");
  #endif
  digitalWrite(RECHTS_VOR, LOW);   // alle Antriebs-Pins deaktieren
  digitalWrite(LINKS_VOR,  LOW);
  digitalWrite(RECHTS_REV, LOW);
  digitalWrite(LINKS_REV,  LOW);
//  digitalWrite(SAUGER,     LOW);
}

// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
void VOR()                         // Antriebsmotoren VORWÄRTS
{
  #ifdef send
    Serial.println("vor");
  #endif
  
  digitalWrite(RECHTS_REV, LOW);   // Rückwärts-Pins auf jeden Fall deaktivieren
  digitalWrite(LINKS_REV,  LOW);
  
  if (KANTE > 0)                   // Kantenmodus
  {
    if (LI_RE < 1)                 // rechts (0)
    {
      analogWrite(RECHTS_VOR,150); // Motor einseitig SLOW für Kurve (immer zur Wand)
      digitalWrite(LINKS_VOR,HIGH);
    }
    else                           // nach links muss dieses Rad Vollgas fahren
    {
      digitalWrite(RECHTS_VOR,HIGH);
      analogWrite(LINKS_VOR,150);  // Motor einseitig SLOW für Kurve (immer zur Wand)
    }
  }
  else                             // Normalmodus mit Sanftanlauf
  {
    ANLAUF = 50;                    // Sanftanlauf (Interrupt-konform)
    while (ANLAUF < 255 && AKTIV > 0)
    {
      analogWrite(RECHTS_VOR, ANLAUF);
      analogWrite(LINKS_VOR,  ANLAUF);
      ANLAUF += 3;
      delay(20);
    }
  }
}

// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
void BACK()                        // Antriebsmotoren rückwärts
{
  digitalWrite(RECHTS_VOR, LOW);   // Vorwärts-Pins auf jeden Fall deaktivieren
  digitalWrite(LINKS_VOR,  LOW);
  
  digitalWrite(RECHTS_REV, HIGH);  // Rückwärts-Pins aktivieren
  digitalWrite(LINKS_REV,  HIGH);
}

// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
void RECHTS()                      // nach RECHTS drehen
{
  digitalWrite(RECHTS_VOR, LOW);
  digitalWrite(LINKS_REV,  LOW);
  
  digitalWrite(RECHTS_REV, HIGH);
  digitalWrite(LINKS_VOR,  HIGH);
}

// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
void LINKS()                       // nach LINKS drehen
{
  digitalWrite(RECHTS_REV, LOW);
  digitalWrite(LINKS_VOR,  LOW);
  
  digitalWrite(RECHTS_VOR, HIGH);
  digitalWrite(LINKS_REV,  HIGH);
}

// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
void SIGNAL_ALARM()                  // Tonfolge für Alarm abgeben
{
  for (long y=0;y<8;y++)
  {
    tone(BEEP, 1000);
    delay(50);
    tone(BEEP,  800);
    delay(50);
  }
  noTone(BEEP);
}

// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
void SIGNAL_INFO()                  // Tonfolge für Info
{
  for (long y=0;y<10;y++)
  {
    tone(BEEP, 1500);
    delay(20);
    tone(BEEP, 1700);
    delay(20);
    tone(BEEP, 1900);
    delay(20);
  }
  noTone(BEEP);
}

// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
void SIGNAL_TIMEOUT()               // Tonfolge bei Blockade-Timeout
{
  for (long y=1000;y<1500;y++)
  {
    tone(BEEP, y);
    delay(2);
  }
  noTone(BEEP);
}

// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
void loop() 
{
  // ~~~~~~~~~~ Testmodus für bestimmte Funktionen, wenn TEST>0 ist ~~~~~~~~~~
  while (TEST > 0)
  // Testbetrieb ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  {
  }
  // ~~~~~~~~~~~~ Ende Testbetrieb ~~~~~~~~~~~~~~~
  
  // Normalbetrieb ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  SPANNUNG = analogRead(AnalogPin); // Akkuspannung messen
  #ifdef send
    Serial.println(SPANNUNG);
  #endif
  if (SPANNUNG < Umin)            // ist die Spannung kleiner als der minimal zulässige Wert Umin ?
  {
    STOP();
    digitalWrite(SAUGER,     LOW);
    
    DAUER = 30;                   // 30 mal im Abstand von 10 Sekunden (insgesamt also =5 min) Signal geben, dass Akku (fast) leer ist - danach SHUTDOWN
    while (DAUER > 0)             // auch bei möglichem 'Spannungspumpen' bei schwachem Akku nicht mehr weitermachen!
    { 
      SIGNAL_ALARM();             // akustisches Signal senden
      delay(10000);
      DAUER--;
    }
    digitalWrite(POWER, LOW);     // Transistor der Spannungs-Versorgungs deaktivieren
    // **************************
    // ****** Betriebsende ******
    // **************************
  } // SPANNUNG < Umin
  
  // Zur Bewältigung ansonsten unerkannter 'Hänger' (also Timeout ohne Kollision) dient die ZEIT (aus millis())
  if (millis() - ZEIT > 30000)    // länger als 30 Sekunden nix passiert... Robbie hängt wahrscheinlich fest
  {
    STOP();
    digitalWrite(SAUGER,     LOW);
    
    TIMEOUT++ ;                   // erneuter Timeout!
    if (TIMEOUT > 1)              // vermutlich dauerhafter Hänger ohne Befreiung eingetreten (z.B. Türschwelle etc.)
      {
      for (long z=0;z<100;z++)    // 100 mal Tröten
        {
        SIGNAL_TIMEOUT();
        delay(1000);
        }
        
        // Neustart erforderlich
        digitalWrite(POWER, LOW); // Transistor der Spannungs-Versorgungs deaktivieren
        // **************************
        // ****** Betriebsende ******
        // **************************
      }
    else
      {
      SIGNAL_INFO();                // Signal ertönen lassen
      delay(500);
      SIGNAL_INFO();
      
      BACK();                        // Versuch der Befreiung
      delay(1000);
      STOP();
      // zufällige ungefähre Komplettdrehung
      if (random(4) > 2)            // zufällig zu einer Seite drehen
      {
        RECHTS();
      }
      else
      {
        LINKS();
      }
      delay(2000);                  // 2 Sekunden zur Seite drehen
      STOP();
      AKTIV = 1;                    // wieder Starten
      
      ZEIT   = millis();            // Zeit-Kontrolle neu setzen
      
      // Modus wechseln, um Wiederholung der Blockade möglichst zu verhindern
      if (KANTE > 0)                // Kantenmodus beenden
      {
        KANTE   = 0;                // Normalmodus aktivieren
        MOD_CNT = 0;
      }
      else                          // Normalmodus beenden
      {
        KANTE   = 1;                // Kantenmodus aktivieren
        MOD_CNT = 51;
      }
    } // millis() - ZEIT > 30000
  }
  switch (AKTIV)
  {
  case 0:                         // Kollision ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
    {
    #ifdef send
      Serial.println("Kollision !!!");
    #endif
    STOP();
    TIMEOUT = 0;                  // Blockade-Timeout nach einer Kollision zurücksetzen, offenbar kein Hänger 'in der Luft'
    ZEIT = millis();              // es wird jetzt eine Befreiungsaktion gestartet, also kann ENDLOS-Wächter auf jeden Fall zurückgesetzt werden
    // ACHTUNG!! Hier muss zwei mal BACK() mit 'delay' gestartet werden, sonst funzt das Ganze nicht !!! ABER WARUM ???
    BACK();                       // Rückwärts
    delay(300);                   // ... nur ein Stückchen
    
    if (KANTE < 1)                // im Normalmodus
      {
      STOP();
      BACK();                     // nochmal etwas Rückwärts
      DAUER = random(300) + 300;
      delay(DAUER);               // ... wieder nur ein zufälliges Stückchen
      }
    STOP();
    
    // jetzt den Kollisions-Bumper direkt abfragen, bis er wieder frei ist
    while (digitalRead(BUMPER) < 1)
      {
      #ifdef send
        Serial.println("Bumper");
      #endif
       
      digitalWrite(SAUGER,     LOW);
      BACK();                      // Rückwärts
      DAUER = random(300) + 300;
      delay(DAUER);                // ... immer wieder nur ein Stückchen
      
      STOP();                      // Antrieb stoppen ... und Bumper wieder abfragen
      }
    
    // Kollisions-Sensor ist wieder frei, es kann also normal weitergehen mit einer Drehung  zur Seite
    attachInterrupt(digitalPinToInterrupt(BUMPER), isr, HIGH); // Interrupt-Routine für BUMPER-Verarbeitung wieder aktivieren
    --COUNTER;                     // Links/Rechts-Umschaltzähler dekrementieren
    #ifdef send
      Serial.println("Wendezähler:");
      Serial.println(COUNTER);
    #endif
    if (COUNTER < 1)               // Umschaltzähler abgelaufen, also neu starten ...
      {
      COUNTER = random(30+KANTE*100)+20; // im Kantenmodus mehr(!) Kollisionen bis zu einem Richtungswechsel zulassen
      LI_RE = -LI_RE;              // ... und Wechsel zwischen links/rechts
      if (KANTE > 0)               // im Kantenmodus nicht 'LI_RE' umkehren, sondern immer in Normalmodus wechseln
        {
        KANTE   = 0;               // Kantenmodus beenden
        MOD_CNT = 0;               // Moduszähler zurücksetzen
        }
      #ifdef send
        Serial.println("Links/Rechts:");
        Serial.print(LI_RE);
      #endif
      }
      
    if (LI_RE > 0)
      {
      RECHTS();                    // LI_RE > 0 : Wendungen nach RECHTS
      }
    else
      {
      LINKS();                     // LI_RE = 0 : Wendungen nach LINKS
      }
    
    DAUER = random(1000) + 200;    // Drehungsdauer zunächst auf einen Random-Wert setzen für unterschiedlich weite Drehungen
    MOD_CNT++;                     // Moduswechselzähler für Umschaltung zwischen Normal/Kante erhöhen (Normalmodus: 1..50, Kantenmodus:51..130)
    if (MOD_CNT > 50)              // Kantenmodus aktiv!
      { 
      DAUER = 300;                 // im Kantenmodus definierte, nur kurze Drehung anstatt vorbelegtem Normalmodus-Randomwert (von weiter oben)
      KANTE = 1; 
      }
    delay(DAUER);                  // Drehungsphase abwarten (Random im Normalmodus und 200msec im Kantenmodus)
    
    if (MOD_CNT > 130)             // Kantenmodus erledigt? Dann wieder in den Normalmodus wechseln
      {
      MOD_CNT = 0;                 // Moduszähler zurücksetzen
      KANTE   = 0;                 // Kantenmodus zurücksetzen auf Normalmodus
      SIGNAL_INFO();               // Akustisches Signal dokumentiert den Wechsel
      }
    STOP();                        // Drehung beenden
    AKTIV = 1;                     // wieder in Putzmodus wechseln
    break;
    }
  case 1:                          // Starten ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
    {
    digitalWrite(SAUGER, HIGH);
    delay(500);                    // Sauger anlaufen lassen
    AKTIV = 2;
    break;
    }
  case 2:                          // Arbeiten ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
    {
    VOR();                         // Arbeitsgang, Saugbetrieb wurde bereits aktiviert
    AKTIV = 99;                    // ab jetzt keine weiteren Aktionen, außer es tritt eine Kollision ein
    break;
    }
  }
  
  // Sicherheitshalber den Bumper auch immer noch direkt abfragen, falls Interrupt nicht 'anspringt'
  if (digitalRead(BUMPER) < 1)     // ist Bumper-Sensor (dauerhaft) ausgelöst?
    {
    AKTIV = 0;                     // Kollisionsmodus aktivieren
    }
}

// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
// (I)nterrupt (S)ervice (R)outine
void isr()                         // Interrupt: BUMPER-Sensor hat ausgelöst
{
  detachInterrupt(BUMPER);         // Kollision wurde erkannt, ab jetzt ohne Interrupt direkt mit dem Sensor arbeiten
  STOP();
  AKTIV = 0;                       // Lauf-Variable deaktivieren
  pinMode(BUMPER,INPUT);           // Pin2 ab jetzt direkt abfragen
}

// EOC ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//