1  component steptest """\
 2  Used by Stepconf to allow testing of acceleration and velocity values for an axis.""";
 3  pin in bit jog-minus "Drive TRUE to jog the axis in its minus direction";
 4  pin in bit jog-plus "Drive TRUE to jog the axis in its positive direction";
 5  pin in bit run "Drive TRUE to run the axis near its current position_fb with a trapezoidal velocity profile";
 6  pin in float maxvel "Maximum velocity";
 7  pin in float maxaccel "Permitted Acceleration";
 8  pin in float amplitude "Approximate amplitude of positions to command during 'run'";
 9  pin in s32 dir "Direction from central point to test: 0 = both, 1 = positive, 2 = negative";
10  pin out float position-cmd;
11  pin in float position-fb;
12  pin out bit running;
13  pin out float run-target;
14  pin out float run-start;
15  pin out float run-low;
16  pin out float run-high;
17  pin in s32 pause = 0 "pause time for each end of run in seconds";
18  param rw float epsilon = .001;
19  variable double timer;
20  param r float elapsed "Current value of the internal timer";
21  variable int timer_on;
22  function _;
23  license "GPL";
24  ;;
25  extern double fabs(double);
26  
27  if (timer_on) {
28      timer += fperiod;
29  }
30  elapsed = timer;
31  if(run) {
32      if(!running) {
33          running = 1;
34          run_start = position_fb;
35  
36          if(dir == 2) run_high = run_start;
37          else run_high = run_start + amplitude;
38  
39          if(dir == 1) run_low = run_start;
40          else run_low = run_start - amplitude;
41  
42          position_cmd = run_low;
43      }
44  
45      if(fabs(position_fb - position_cmd) < epsilon) {
46          if ((position_cmd == run_low) || (position_cmd ==run_high)) {
47              if (!timer_on) {
48                  timer = 0;
49                  timer_on = true;
50              } else if (timer >= pause) {
51                  timer_on = false; 
52                  if(position_cmd == run_low) {
53                      position_cmd = run_high;
54                  } else {
55                      position_cmd = run_low;
56                  }
57              }
58          }
59      }
60  } else if(running) {
61      position_cmd = run_start;
62      if(fabs(position_fb - run_start) < epsilon) {
63          running = 0;
64          timer_on = false;
65      }
66  } else {
67      if(jog_minus) {
68          position_cmd = position_fb - 2 * maxvel * fperiod;
69      } else if(jog_plus) {
70          position_cmd = position_fb + 2 * maxvel * fperiod;
71      } else {
72          // Let the command track the feedback while the stepgen decelerates
73          // to a halt, then keep the command constant there.
74          if (fabs(position_fb - position_cmd) > (0.001 * maxvel * fperiod)) {
75              position_cmd = position_fb;
76          }
77      }
78  }