IN[1] y active la cinta mediante OUT[1], depositándolas en POSFIN de forma indefinida.
; ========================================================
; PROGRAMA DE RECOGIDA CONTINUA EN CINTA TRANSPORTADORA
; ========================================================
LABEL 10 ; Bucle principal indefinido
SET OUT[1] = 1 ; Arrancar motor de la cinta transportadora
LABEL 20 ; Bucle de espera activa de pieza
IF IN[1] == 0 GOTO 20 ; Si no hay pieza detectada por el sensor óptico, esperar
SET OUT[1] = 0 ; Parar la cinta inmediatamente (la pieza está en POSINI)
OPEN 1 ; Abrir pinza del robot
MOVE POSAPPR ; Posición de aproximación sobre la cinta (evita colisiones)
MOVE POSINI ; Posicionarse sobre la pieza
CLOSE 1 ; Cerrar pinza y sujetar la pieza
DELAY 20 ; Pausa de 200 ms para asegurar agarre neumático
MOVE POSAPPR ; Retirar la pieza verticalmente
MOVE POSFIN_APPR ; Aproximación a la zona de destino
MOVE POSFIN ; Bajar a la posición de descarga
OPEN 1 ; Soltar la pieza
MOVE POSFIN_APPR ; Retirar herramienta verticalmente
GOTO 10 ; Repetir ciclo indefinidamente para la siguiente pieza| Configuración | Cinemática y Accionamiento | Ventajas | Desventajas |
|---|---|---|---|
| Diferencial (2 motrices + 1 loca) | $\omega = \frac{v_d - v_i}{L}$, $v = \frac{v_d + v_i}{2}$ | Giro sobre su propio centro geométrico ($R=0$). Sencillez mecánica y cinemática elemental. | Sensible a deslizamientos laterales. Rueda loca genera vibraciones en cambios de sentido. |
| Triciclo Clásico (1 directriz motriz + 2 fijas) | Una rueda delantera con tracción y dirección; dos traseras libres no orientables. | Control de tracción y dirección centralizado en una sola rueda. Excelente tracción delantera. | Radio de giro limitado ($R > 0$). Inestabilidad de vuelco si el centro de gravedad está alto. |
| Tipo Automóvil / Ackerman | 2 ruedas directrices acopladas (trapecio de Ackerman) + 2 ruedas motrices traseras. | Altísima estabilidad dinámica a velocidades elevadas. Comportamiento idéntico al del coche. | Cinemática no holónoma restrictiva. No puede girar sobre sí mismo ($R_{\min} \gg 0$). Maniobras complejas de aparcamiento. |
| Omnidireccional (Mecanum / Omniwheels) | 3 o 4 ruedas con rodillos pasivos a $45^\circ$ o $90^\circ$ sobre su periferia. | Holonómico: 3 GDL instantáneos ($v_x, v_y, \omega$). Se desplaza en cualquier dirección sin rotar previamente. | Baja adherencia sobre superficies irregulares. Mayor coste y complejidad de control multieje. |
MOVE P1) y valida límites de espacio de trabajo y sintaxis.| Método | C1: Resolución / Precisión | C2: Frecuencia de Medida | C3: Coste Económico |
|---|---|---|---|
| M1: Odometría con encoders | Muy alta a corto plazo (submilimétrica), pero deriva acumulativa ilimitada por patinamiento. | Muy alta ($100\text{ Hz} - 1\text{ kHz}$), lectura inmediata de pulsos en el microcontrolador. | Muy bajo (sensores integrados en los motores de las ruedas). |
| M2: GPS Absoluto Comercial | Baja (error típico entre $2\text{ m}$ y $5\text{ m}$ en exteriores, nulo en interiores). | Baja ($1\text{ Hz} - 10\text{ Hz}$). | Bajo (receptores estándar muy económicos). |
| M3: GPS Diferencial (DGPS / RTK) | Muy alta y sin deriva ($1\text{ cm} - 2\text{ cm}$) gracias a la estación base de referencia fija. | Media ($5\text{ Hz} - 20\text{ Hz}$). | Alto / Muy alto (requiere estación base propia o suscripción a redes GNSS activas). |
En el algoritmo hacia adelante (*forward recursion*) de Newton-Euler, la velocidad angular de un eslabón $i$ se propaga como: $$\boldsymbol{\omega}_i = \boldsymbol{\omega}_{i-1} + \dot{\theta}_i \mathbf{z}_{i-1}$$ Derivando temporalmente respecto a un sistema inercial de referencia: $$\boldsymbol{\alpha}_i = \frac{d\boldsymbol{\omega}_i}{dt} = \frac{d\boldsymbol{\omega}_{i-1}}{dt} + \frac{d}{dt}(\dot{\theta}_i \mathbf{z}_{i-1}) = \boldsymbol{\alpha}_{i-1} + \ddot{\theta}_i \mathbf{z}_{i-1} + \dot{\theta}_i \left(\frac{d\mathbf{z}_{i-1}}{dt}\right)$$ Dado que el eje $\mathbf{z}_{i-1}$ es solidario al eslabón $i-1$, su variación temporal se debe a la rotación de dicho eslabón: $\frac{d\mathbf{z}_{i-1}}{dt} = \boldsymbol{\omega}_{i-1} \times \mathbf{z}_{i-1}$. Por consiguiente:
Proyectando en los ejes del propio sistema de coordenadas del eslabón $\{i\}$: $$\boldsymbol{\alpha}_i^{(i)} = \mathbf{R}_i^{i-1, T} \left( \boldsymbol{\alpha}_{i-1}^{(i-1)} + \boldsymbol{\omega}_{i-1}^{(i-1)} \times (\dot{\theta}_i \mathbf{z}_0) + \ddot{\theta}_i \mathbf{z}_0 \right)$$ donde $\mathbf{z}_0 = [0, 0, 1]^T$. (Para articulación prismática $\dot{\theta}_i = \ddot{\theta}_i = 0 \implies \boldsymbol{\alpha}_i^{(i)} = \mathbf{R}_i^{i-1, T} \boldsymbol{\alpha}_{i-1}^{(i-1)}$).
Un sistema $XYZ$ se transforma en $UVW$ mediante un giro $\alpha = 30^\circ$ en torno al eje $Z$, seguido de un desplazamiento a lo largo del nuevo eje $X$ de $d = 1\text{ m}$.
La matriz homogénea de cambio de base que expresa puntos de $UVW$ en $XYZ$ es:
$$T_{XYZ}^{UVW} = \text{Rot}(Z, 30^\circ) \cdot \text{Tras}(X, 1) = \begin{bmatrix} \cos 30^\circ & -\sin 30^\circ & 0 & \cos 30^\circ \cdot 1 \\ \sin 30^\circ & \cos 30^\circ & 0 & \sin 30^\circ \cdot 1 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} = \begin{bmatrix} \frac{\sqrt{3}}{2} & -\frac{1}{2} & 0 & \frac{\sqrt{3}}{2} \\ \frac{1}{2} & \frac{\sqrt{3}}{2} & 0 & \frac{1}{2} \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}$$
Para el punto $p^{(UVW)} = [0, 1, -1]^T$:
$$\begin{bmatrix} x \\ y \\ z \\ 1 \end{bmatrix} = \begin{bmatrix} \frac{\sqrt{3}}{2} & -\frac{1}{2} & 0 & \frac{\sqrt{3}}{2} \\ \frac{1}{2} & \frac{\sqrt{3}}{2} & 0 & \frac{1}{2} \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} 0 \\ 1 \\ -1 \\ 1 \end{bmatrix} = \begin{bmatrix} -\frac{1}{2} + \frac{\sqrt{3}}{2} \\ \frac{\sqrt{3}}{2} + \frac{1}{2} \\ -1 \\ 1 \end{bmatrix} \approx \begin{bmatrix} 0.366 \\ 1.366 \\ -1.0 \\ 1 \end{bmatrix}$$
Comandos en MATLAB:
T = trotz(deg2rad(30)) * transl(1, 0, 0); p_xyz = T * [0; 1; -1; 1];