component spindle_monitor "spindle at-speed and underspeed detection";
pin in bit spindle-is-on;
pin in float spindle-command;
pin in float spindle-feedback;

pin out bit spindle-at-speed;
pin out bit spindle-underspeed;

param rw unsigned level "state machine state";
param rw float threshold;

function _;
license "gpl v2 or higher";
author "Sebastian Kuzminsky";

;;

#include <rtapi_math.h>

FUNCTION(_) {

switch (level){
    case 0:  // idle
        spindle_at_speed = 0;
        spindle_underspeed = 0;
        if (spindle_is_on) level = 1;
        break;
    case 1: // waiting for spindle-at-speed
        if ( ! spindle_is_on ) {
            level = 0;
            return; }
        if (fabs(spindle_command - spindle_feedback) < threshold) {
            level = 2;
            spindle_at_speed = 1;
            return; }
        break;
    case 2: // monitoring speed
        if ( ! spindle_is_on ) {
            level = 0;
            return; }
        if  ((spindle_command - spindle_feedback) > threshold) {
            spindle_underspeed = 1; }
        break;
    default:
        // not sure how we got here, but fix the situation
        level = 0;
    }
}