Pick-and-Place-Zustandsmaschine, die Fehler überlebt

Warum die Demo klappt und das System nicht

Jedes erste Pick-and-Place ist eine Liste von delay()-Aufrufen. Fahre hierhin, schließe den Greifer, fahre dorthin, öffne ihn. Auf Video klappt es, und auf dem Tisch klappt es etwa achtzig von hundert Mal — was sich fast fertig anfühlt.

Es ist nicht fast fertig, denn die zwanzig Fehler sind alle lautlos. Der Greifer schloss sich um nichts, und der Arm trug Luft über den Tisch und öffnete über dem Behälter. Das Teil lag leicht schräg und wurde umgekippt statt gegriffen. Das Servo bekam beim Heben einen Spannungseinbruch, und die Steuerung startete neu in unbekannter Pose mit geschlossenen Backen.

Was daraus Bergungen statt Katastrophen macht, ist eines: ein Prüfschritt zwischen Greifen und Heben und eine Zustandsmaschine, die einen Ort hat, wohin sie bei Fehlschlag gehen kann. Darum geht es auf dieser Seite.

Die Zustände

Ein Pick-and-Place-Zyklus sind neun Zustände, und es lohnt sich, alle neun zu benennen statt sie zusammenzufassen.

Zwei Entwurfsentscheidungen leisten hier die meiste Arbeit.

Anfahr- und Rückzugsposen sind eigene Zustände, über dem Objekt. Ein Greifer, der aus beliebiger Richtung direkt auf ein Teil sticht, kippt es um, und einer, der entlang einer Kurve hebt, zieht das Teil seitlich weg, bevor es frei ist. Komme immer direkt von oben (oder direkt entlang der Fügensachse) mit kurzem geradem Segment, und verlasse es genauso.

VERIFY ist ein Zustand, keine Codezeile. Es ist die einzige Stelle im Zyklus, wo die Maschine erfährt, ob die Wirklichkeit zum Plan passte — und sie braucht einen Ort, wohin sie bei Fehlschlag gehen kann.

Den Griff prüfen

Vier Wege, billigster zuerst. Du willst mindestens einen.

Die Backenpositionsprüfung (gratis)

Wenn dein Greifer nachgiebig ist, stoppt ein Griff um ein Objekt die Backen vor der Position, die sie beim Schließen in Luft erreichen. Kannst du die Backenposition lesen, sagt dir der Vergleich mit der Leer-Schließposition, dass etwas zwischen den Fingern ist.

An einem normalen Hobbyservo kannst du die Position nicht lesen — er ist offen geregelt. Aber ein Servo, dessen Potenziometerabgriff auf einen ADC-Pin geführt ist, gibt dir das für den Preis eines Drahts, und dieser Umbau dauert zehn Minuten.

Grenzen: Es sagt dir, dass etwas da ist, nicht dass es das Richtige ist oder gut gehalten wird.

Ein Mikroschalter am Finger (ca. 50 Cent)

Ein Hebel-Mikroschalter an der Innenfläche eines Fingers, verdrahtet an einen Eingangspin mit Pull-up. Schließt, wenn die Backe etwas berührt.

Das ist mit Abstand die preiswerteste Option. Es ist eine direkte digitale Antwort auf „ist etwas zwischen den Backen“, es überlebt alles, und es entprellt sich in Software in drei Zeilen. Setze es auf den nachgiebigen Teil des Fingers, damit es bei wiederholbarer Kraft zuverlässig schaltet.

Strommessung (ca. 2 Euro)

Ein Shunt oder Hallsensor in der Servoversorgung. Ein Servo, das in Luft schließt, regelt auf Leerlauf herunter; eines, das auf ein Objekt schließt, bleibt bei erhöhtem Strom.

Grenzen: verrauscht, braucht eine kalibrierte Schwelle, und geht schief, wenn die Batterie einbricht. Lohnt sich, wenn du auch ein Zerdrücken erkennen willst statt nur Halten — ein Strom weit über dem Erwartungswert heißt, das Teil ist härter als erwartet oder du hast danebengegriffen und die Vorrichtung gepackt.

Lichtschranke über den Backen (ca. 1 Euro)

Ein IR-Sender und Fototransistor über der Backenöffnung. Unterbrochener Strahl heißt, etwas ist im Spalt.

Grenzen: Sie sieht das Objekt, bevor du es gegriffen hast, prüft also Anwesenheit statt Griff. Kombiniere sie mit etwas anderem. Ehrlich gut an Maschinen, die transparente oder dunkle Objekte greifen, wo Bildverarbeitung kämpft.

Es aufschreiben

Das Muster ist eine schlichte endliche Zustandsmaschine mit nicht-blockierendem Timer, damit nichts in der Schleife stehen bleibt und der Not-Halt reaktionsfähig bleibt:

enum State { IDLE, APPROACH, DESCEND, GRASP, VERIFY, LIFT, TRANSFER, PLACE, RECOVER };

State state = IDLE;
uint32_t stateEnteredAt = 0;
uint8_t  attempts = 0;

const uint8_t  MAX_ATTEMPTS = 3;
const uint16_t GRASP_DWELL_MS = 350;   // let the compliance settle before you trust it
const uint16_t SETTLE_MS      = 250;

void enter(State next) { state = next; stateEnteredAt = millis(); }
bool elapsed(uint16_t ms) { return millis() - stateEnteredAt >= ms; }

// Debounced: one reading is a glitch, three in a row is a fact.
bool objectHeld() {
  static uint8_t streak = 0;
  streak = digitalRead(GRIP_SWITCH) == LOW ? min(streak + 1, 3) : 0;
  return streak >= 3;
}

void loop() {
  arm.update();          // non-blocking motion, every pass
  gripper.update();

  switch (state) {
    case IDLE:
      if (jobPending()) { attempts = 0; arm.moveTo(approachPose); enter(APPROACH); }
      break;

    case APPROACH:
      gripper.open();
      if (arm.arrived() && elapsed(SETTLE_MS)) { arm.moveTo(graspPose); enter(DESCEND); }
      break;

    case DESCEND:
      if (arm.arrived()) { gripper.close(); enter(GRASP); }
      break;

    case GRASP:
      // The dwell is not politeness. A compliant finger rings after closing,
      // and reading the switch during the ring gives you a coin flip.
      if (elapsed(GRASP_DWELL_MS)) enter(VERIFY);
      break;

    case VERIFY:
      if (objectHeld()) { arm.moveTo(approachPose); enter(LIFT); }
      else              { enter(RECOVER); }
      break;

    case LIFT:
      if (arm.arrived()) { arm.moveTo(placeApproach); enter(TRANSFER); }
      break;

    case TRANSFER:
      if (arm.arrived()) { arm.moveTo(placePose); enter(PLACE); }
      break;

    case PLACE:
      if (arm.arrived() && elapsed(SETTLE_MS)) {
        gripper.open();
        // Open BEFORE retreating, and retreat straight up, or you drag the part.
        if (elapsed(SETTLE_MS + 200)) { arm.moveTo(placeApproach); enter(IDLE); }
      }
      break;

    case RECOVER:
      gripper.open();
      if (elapsed(SETTLE_MS)) {
        if (++attempts < MAX_ATTEMPTS) { arm.moveTo(approachPose); enter(APPROACH); }
        else { raiseFault("grasp failed after 3 attempts"); enter(IDLE); }
      }
      break;
  }
}

Drei Details darin verdienen ihren Platz:

Die Greif-Verweilzeit. Ein nachgiebiger Finger schwingt nach dem Schließen ein paar hundert Millisekunden nach. Während des Nachschwingens zu prüfen gibt einen Münzwurf. 300–400 ms reichen meist; miss deins, indem du den Schalter eine Sekunde nach dem Schließen loggst.

Entprellte Prüfung. Eine Schalterlesung ist ein Glitch. Drei aufeinanderfolgende Lesungen sind eine Tatsache, und sie kosten drei Durchläufe einer Schleife, die mit hunderten Hertz läuft.

Begrenzte Wiederholung. Ewig erneut zu versuchen ist, wie ein Roboter die Nacht damit verbringt, an einem Teil zu mahlen, das vor einer Stunde auf den Boden fiel. Drei Versuche, dann ein Fehler, den ein Mensch quittieren muss.

Was tun, wenn der Wiederholversuch nicht hilft

RECOVER, wie geschrieben, versucht es nur erneut aus derselben Pose — was den Normalfall fixt: Das Teil lag leicht daneben, die Backen stupsten es an, und der zweite Versuch sitzt. Es fixt kein Teil, das ganz woanders ist. Nützliche Eskalationen, der Reihe nach:

1. Anfahrt verrücken. Die Greifpose bei jedem Wiederholversuch um ein paar Millimeter in kleiner Spirale versetzen. Kostet drei Zeilen und rettet überraschend viele Fälle.
2. Neu fühlen. Wenn du eine Kamera oder einen Distanzsensor hast, nimm eine frische Messung statt der Pose zu vertrauen, die man dir gab.
3. Öffnung erweitern. Eine Backenöffnung knapp ums Nennteil versagt an einem verdrehten Teil. Beim Wiederholen weiter öffnen.
4. Laut aufgeben. Ein Fehler, der die Maschine anhält und sagt, in welchem Zustand er war, schlägt eine Maschine, die weiterläuft und leise den Ausschussbehälter füllt.

Die Fehler, die nicht am Greifer liegen

Wenn ein Pick-and-Place unzuverlässig ist, liegt es oft nicht am Griff:

Die vierte Zeile verdient Verweilen, weil sie am meisten wie ein Rätsel aussieht. Ein Griff, der auf dem Tisch perfekt hält, kann 40 ms nach Hubbeginn loslassen — bevor der Arm sich sichtbar bewegt hat — weil der Beschleunigungsanteil dominiert. Der Greifer-Simulator zeigt dir den exakten Moment für deine Zahlen, und der Fix ist meist, das Profil zu verlangsamen statt fester zu quetschen.

Wie weiter

Sobald der Zyklus zuverlässig ist, die nächsten lohnenden Ergänzungen, nach Wert sortiert:

1. Zyklenzählung und Fehlerprotokoll. Eine Zuverlässigkeitszahl, die du nicht misst, kannst du nicht verbessern.
2. Ein sauberes Bewegungsprofil auf dem Transfer, damit die Beschleunigung begrenzt ist und du den Griff nicht mehr überdimensionierst, um ruckartige Fahrten zu überleben.
3. Pose aus Sensorik, nicht aus Konstanten — das ist der Schritt von der Demo zur Maschine.

Weiter

• Endliche Zustandsmaschinen für Roboterverhalten — das allgemeine Muster, dessen Instanz dies ist
• Servogreifer-Kraft, Blockieren und Nachgiebigkeit — warum es die Verweilzeit gibt und warum die Backen nicht geschlossen bleiben sollten
• Robotergreifertypen und Greifkraft — Bemessung des Griffs, von dem die Sequenz abhängt
• Der Greifer-Simulator — finde den Moment, in dem ein Griff bei deinen Zahlen loslässt