Máquina de estados para Pick-and-Place tolerante a fallos

Por qué la demo funciona y el sistema no

El primer pick-and-place de cualquiera es una lista de llamadas a delay(). Muévete aquí, cierra la pinza, muévete allá, ábrela. Funciona en vídeo y funciona unas ochenta veces de cada cien en el banco, lo que parece lo bastante cerca de terminado.

No está cerca de terminado, porque los veinte fallos son todos silenciosos. La pinza se cerró sobre nada y el brazotransportó aire por encima de la mesa y abrió sobre la bandeja. La pieza estaba un poco inclinada y la tumbó en vez de agarrarla. El servo se se quedó sin tensión a media elevación y el controlador se reinició en una pose desconocida con las mandíbulas cerradas.

Lo que convierte esos desastres en recuperaciones es una sola cosa: un paso de verificación entre agarrar y levantar, y una máquina de estados que tiene adónde ir cuando falla. Ese es todo el tema de esta página.

Los estados

Un ciclo de pick-and-place son nueve estados, y merece la pena nombrar los nueve en vez de agruparlos.

Dos puntos de diseño hacen casi todo el trabajo aquí.

Las poses de aproximación y retirada son estados separados, por encima del objeto. Una pinza que se precipita en línea recta contra una pieza desde una dirección arbitraria la tumba, y una pinza que levanta siguiendo una curva arrastra la pieza de lado antes de que se libere. Entra siempre por directamente encima (o directamente sobre el eje de inserción) con un segmento recto corto, y sal igual.

VERIFY es un estado, no una línea de código. Es el único punto del ciclo donde la máquina descubre si la realidad coincidió con el plan, y necesita un sitio al que poder fallar.

Verificar el agarre

Cuatro maneras, de la más barata a la más cara. Quieres al menos una.

La comprobación de la posición de las mandíbulas (gratis)

Si tu pinza tiene algo de compliancia en serie, un agarre sobre un objeto detiene las mandíbulas antes de donde llegan al cerrarse sobre el aire. Si puedes leer la posición de las mandíbulas, compararla con la posición de cierre en vacío te dice que hay algo entre los dedos.

En un servo de hobby normal no puedes leer la posición — es de lazo abierto. Pero un servo con el cursor del potenciómetro llevado a un pin ADC te da esto por el precio de un cable, y esa modificación lleva diez minutos.

Límites: te dice que hay algo ahí, no que sea lo correcto ni que esté bien sujeto.

Un microinterruptor en el dedo (unos 50 peniques)

Un microinterruptor de palanca en la cara interior de uno de los dedos, cableado a un pin de entrada con pull-up. Cierra cuando la leva toca algo.

Es la opción con mejor relación calidad-precio por goleada. Es una respuesta digital directa a «¿hay algo entre las mandíbulas?», aguanta todo, y hace el antirebote en software en tres líneas. Ponlo en la parte compliante del dedo para que se dispare de forma fiable a una fuerza repetible.

Sensorización de corriente (unos 2 £)

Un derivador o un sensor de efecto Hall en la alimentación del servo. Un servo que cierra sobre el aire baja su corriente a la de reposo; un servo que cierra sobre un objeto se mantiene en corriente elevada.

Límites: es ruidoso, necesita un umbral que calibras, y falla cuando la batería se cae. Vale la pena cuando además quieres detectar un aplastamiento y no solo una sujeción — una corriente muy por encima del valor esperado significa que la pieza es más dura de lo que esperabas, o que fallaste y agarraste el utillaje.

Corte de haz entre las mandíbulas (unos 1 £)

Un emisor de infrarrojos y un fototransistor cruzando la apertura de las mandíbulas. Haz cortado significa que hay algo en el hueco.

Límites: ve el objeto antes de que lo hayas agarrado, así que verifica presencia en vez de agarre. Combínalo con otra cosa. Genuinamente bueno en máquinas que cogen objetos transparentes u oscuros donde a la visión le cuesta.

Cómo escribirlo

El patrón es una máquina de estados finita normal con un temporizador no bloqueante, así que nada en el bucle se atasca y el paro de emergencia sigue respondiendo:

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;
  }
}

Tres detalles de ahí se ganan su sitio:

La espera de agarre. Un dedo compliante rebaba durante un par de cientos de milisegundos después de cerrarse. Verifica durante el rebabado y obtienes una moneda al aire. 300–400 ms suele bastar; mide el tuyo registrando el interruptor durante un segundo tras un cierre.

Verificación con antirebote. Una lectura del interruptor es un fallo puntual. Tres lecturas consecutivas son un hecho, y cuesta tres pasadas de un bucle que corre a cientos de hercios.

Un reintento acotado. Reintentar para siempre es cómo un robot se pasa la noche rozando una pieza que se cayó al suelo hace una hora. Tres intentos y luego un fallo que tiene que limpiar una persona.

Qué hacer cuando el reintento no ayuda

RECOVER tal como está escrito solo lo intenta otra vez desde la misma pose, lo que arregla el caso común — la pieza estaba un poco desplazada, las mandíbulas la tocaron, y el segundo intento acierta. No arregla una pieza que está en otro sitio del todo. Escalaciones útiles, por orden:

1. Desplaza la aproximación. Desplaza la pose de agarre un par de milímetros en una espiral pequeña en cada reintento. Cuesta tres líneas y recupera una fracción sorprendente.
2. Vuelve a medir. Si tienes cámara o sensor de distancia, toma una lectura nueva en vez de fiarte de la pose que te dieron.
3. Abre más la apertura. Una apertura de mandíbulas ajustada a la pieza nominal falla con una pieza girada. Abre más en el reintento.
4. Ríndete en voz alta. Un fallo que para la máquina y dice en qué estado estaba es mejor que una máquina que sigue y va llenando en silencio la bandeja de rechazo.

Los fallos de los que no es culpable la pinza

Cuando un pick-and-place es poco fiable, a menudo no es el agarre:

La cuarta fila merece detenerse, porque es la que más parece un misterio. Un agarre que sujeta perfectamente en el banco puede soltar 40 ms dentro de una elevación — antes de que el brazo se haya movido de forma visible — porque domina el término de aceleración. El simulador de pinza te mostrará el instante exacto para tus números, y el arreglo suele ser frenar el perfil en vez de apretar más.

Hacia dónde seguir

Una vez que el ciclo es fiable, lo siguiente que merece la pena añadir, por orden de valor:

1. Contado de ciclos y registro de fallos. No puedes mejorar una cifra de fiabilidad que no estás midiendo.
2. Un perfil de movimiento de verdad en la transferencia, para que la aceleración esté acotada y dejes de dimensionar la pinza en exceso para sobrevivir a movimientos bruscos.
3. Pose a partir de la percepción, no de constantes, que es el paso de una demo a una máquina.

Siguiente

• Máquinas de estados para el comportamiento de un robot — el patrón general del que este es un caso
• Fuerza, corriente de atasco y compliancia en pinzas de servo — por qué existe la espera y por qué las mandíbulas no deberían quedarse cerradas
• Tipos de pinza robótica y fuerza de agarre — dimensionar el agarre del que depende la secuencia
• El simulador de pinza — encuentra el instante en que un agarre suelta con tus propios números