1  component sim_home_switch "Home switch simulator";
 2  
 3  description
 4  """
 5  After tripping home switch, travel in opposite direction is
 6  required (amount set by the hysteresis pin).
 7  A pin (index-enable) is provided for use when
 8  \\fB[JOINT_n]HOME_USE_INDEX\\fR is specified to reset
 9  the I/O pin \\fBjoint.N.index-enable\\fR.
10  """;
11  pin in float cur_pos "Current position (typically: joint.n.motor-pos-fb)";
12  pin in float home_pos = 1 "Home switch position";
13  pin in float hysteresis = 0.1"Travel required to backoff (hysteresis)";
14  pin out bit  home_sw"Home switch activated";
15  
16  pin io  bit  index_enable "typ: connect to joint.N.index-enable";
17  pin in float index_delay_ms = 10 "delay in msec to reset index-enable";
18  
19  variable int    old_index_enable;
20  variable double index_timer_ms;
21  
22  function _ fp;
23  license "GPL";
24  ;;
25  
26  FUNCTION(_) {
27      // could be simplified but this style is meant to be easy-to-read
28      if (home_pos >= 0) {
29          // home switch is on positive side
30          if (cur_pos >= home_pos) {
31              home_sw = 1;
32          } else {
33              if (cur_pos <= (home_pos - hysteresis) ) {
34                  home_sw = 0;
35              } else {
36                  if (home_sw) {
37                      home_sw = 1;
38                  } else {
39                      home_sw = 0;
40                  }
41              }
42          }
43      } else {
44          // negative home switch location
45          if (cur_pos <= home_pos) {
46              home_sw = 1;
47          } else {
48              if (cur_pos >= (home_pos + hysteresis) ) {
49                  home_sw = 0;
50              } else {
51                  if (home_sw) {
52                      home_sw = 1;
53                  } else {
54                      home_sw = 0;
55                  }
56              }
57          }
58      }
59  
60      // provision to reset I/O pin index-enable
61      if (index_timer_ms > 0) {
62         index_timer_ms -= period * 1e-6; // period is in nS
63         if (index_timer_ms <= 0) {
64             index_timer_ms = 0;
65             index_enable = 0;
66             return;
67         }
68      }
69      if (index_enable && !old_index_enable) {
70          index_timer_ms = index_delay_ms;
71      }
72      old_index_enable = index_enable;
73      return;
74  }