1  component spindle_monitor "spindle at-speed and underspeed detection";
 2  pin in bit spindle-is-on;
 3  pin in float spindle-command;
 4  pin in float spindle-feedback;
 5  
 6  pin out bit spindle-at-speed;
 7  pin out bit spindle-underspeed;
 8  
 9  param rw unsigned level "state machine state";
10  param rw float threshold;
11  
12  function _;
13  license "gpl v2 or higher";
14  
15  ;;
16  
17  #include <rtapi_math.h>
18  
19  FUNCTION(_) {
20  
21  switch (level){
22      case 0:  // idle
23          spindle_at_speed = 0;
24          spindle_underspeed = 0;
25          if (spindle_is_on) level = 1;
26          break;
27      case 1: // waiting for spindle-at-speed
28          if ( ! spindle_is_on ) {
29              level = 0;
30              return; }
31          if (fabs(spindle_command - spindle_feedback) < threshold) {
32              level = 2;
33              spindle_at_speed = 1;
34              return; }
35          break;
36      case 2: // monitoring speed
37          if ( ! spindle_is_on ) {
38              level = 0;
39              return; }
40          if  ((spindle_command - spindle_feedback) > threshold) {
41              spindle_underspeed = 1; }
42          break;
43      default:
44          // not sure how we got here, but fix the situation
45          level = 0;
46      }
47  }