blob: 51f44b93717b6ef4f2edcdcd241a261ec072a137 (
plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
|
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;
}
}
|
