Implementación de un bloque de funciones de preprocesamiento de rutas

El ejemplo muestra una aplicación de corte 2D. Se debe usar un cuchillo para cortar a lo largo de una trayectoria definida por el código G. El SMC_TRAFO_GantryCutter2 La transformación se usa para determinar el ángulo de la cuchilla en función de la tangente actual de la trayectoria.

El desafío es que la cuchilla no debe girar demasiado rápido, de lo contrario el corte no quedará limpio. La limitación de la velocidad angular de la cuchilla se resuelve en el proyecto de muestra mediante el bloque de funciones de preprocesamiento de rutas recién creado LimitTangentVelocity. Cambia la velocidad de avance en la trayectoria para que la velocidad de cambio de la tangente no supere un valor máximo configurable

El código G es un rectángulo simple con esquinas suavizadas.

La traza muestra el resultado de la ejecución: la velocidad del eje C, que corresponde a la velocidad de rotación de la cuchilla, se muestra en naranja. Está limitado a 45 °/s según lo especificado

La estructura es típica de las aplicaciones de CNC. El código G se lee en la tarea en segundo plano y aquí también se realiza el preprocesamiento de la ruta. La interpolación se realiza en

El LimitTangentVelocity el bloque de función muestra los pasos y estados necesarios para un bloque de funciones de preprocesamiento de rutas.

El principio básico es que el bloque de funciones lee, procesa y, a continuación, escribe los elementos de la ruta (tipo SMC_GEOINFO) de la cola de entrada (poqDataIn) a la cola de salida (poqDataOut). Los elementos se eliminan de la cola de entrada

La lógica para cambiar la velocidad de alimentación se encuentra en las líneas 102-138. ComputeMaxCurvature calcula la curvatura máxima que puede producirse en un elemento de trayectoria determinado.

// Our velocity limitation comes here. This modifies an element from the
// input queue and copies the modified element to the output queue.
// Finally, the element is removed from the input queue

m_geo := pgeo^; // Copy the element
 // Note: the feature flags set in the G-Code with G38/G39 can be queried
 // by reading pgo^.dwFeatureFlags, like this:
 IF (SHR(m_geo.dwFeatureFlags, featureFlag) AND 1) = 1 THEN
     // feature is turned on

     ok := ComputeMaxCurvature(m_geo, kappa=> kappa_max);
     IF NOT ok THEN
         // Curvature cannot be computed
         m_state := STATE_ERROR;
         ErrorID := SMC_INVALID_PARAMETER;
         OnExit();
         RETURN;
     END_IF

     // Compute maximum allowed path velocity based on maximum curvature
     // and maximum allowed angular velocity
     IF kappa_max = 0 THEN
         // No curvature, no limitation necessary
         vel := m_geo.dVel;
     ELSE
         vel := m_maxAngularVelocity_rad / kappa_max;
     END_IF

     IF velMin < 0 OR vel < velMin THEN
         velMin := vel;
     END_IF

     // Set new maximum velocity for the element
     m_geo.dVel := MIN(m_geo.dVel, vel);
 ELSE
     // feature is turned off
 END_IF