/*
 * MotorKnob-filter
 *
 * A stepper motor follows the turns of a potentiometer
 * (or other sensor) on analog input 0.
 *
 * http://www.arduino.cc/en/Reference/Stepper
 * This example code is in the public domain.
 *
 * This sketch is a modification of the MotorKnob.ino sketch
 * distributed in the Arduino Stepper library. Modifications made by
 * John Conover, john@email.johncon.com.
 *
 * It adds a pole in the transfer function to reduce "bit chatter"
 * from the Arduino A0 pin A/D, (or noisy sensor,) to reduce "bit
 * chatter" on the stepper motor.
 *
 * The pole is implemented in software in this sketch as a recursive
 * DSP single pole filter. The original source is the tspole(1)
 * program in the fractal.tar.gz tape archive, available from
 * http://www.johncon.com/ndustrix/.
 *
 * The licensing is whatever the licensing of the Arduino
 * MotorKnob.ino sketch is.
 *
 * The approach will be to develop two naive solutions, (which
 * interact with unsatisfactory consequences,) then combine these two
 * solutions for an optimal solution that minimizes the undesirable
 * effects, and, (at the same time,) maximizes the desirable effects,
 * to the same magnitude, for both. (This is a general solution for
 * electronic stability control in mechanical systems-and can be used
 * to provide dominant pole compensation in a closed loop system,
 * where a sensor provides indexed position control signals.)
 *
 * The maximum stepper motor tracking rate is 200 steps per revolution
 * at 50 RPM, or 200 steps in 1/50 minute, or (50 * 200) / 60 = 166
 * steps per second, which is 1 / 166 = 6.024096 mS per step.
 *
 * The tracking Voltage on the Arduino pin A0, (a 10 bit A/D,) is 5 V
 * / 1023 = 4.88759 mV, (~ 10,000 A/D samples per second):
 *
 *     0.00488759 / 0.006024096 = 0.81134 V / S
 *
 * The single pole RC filter step response would be:
 *
 *      Vo = Vi * (1 - e^(- t / (R * C)) = Vi - Vi * e^(-t / (R * C))
 *
 *      dVo / dt = -Vi * (-1 / (R * C)) * e^(-t / (R * C))
 *
 *          which maximizes when t = 0, e^0 = 1
 *
 *      dVo / dt, maximum, = Vi  / (R * C)
 *
 *      5 / (R * C) = 0.81134 V / S
 *      R * C = 5 / 0.81134 = 6.162645
 *
 * The pole frequency, fp, for tracking a 0 to 5V step on Arduino pin
 * A0:
 *
 *     fp = 1 / (2 * pi * R * C) = 1 / (2 * pi * 6.162645)
 *                               = 0.0258258 Hz.
 *
 *          Note that k1 would be 0, and k2 would be 1, making:
 *
 *             int val = (int) ((((double) I * k2) + ((double) previous * k1)) + (double) 0.5);
 *                     = (int) ((((double) I) + (double) 0.5);
 *                     = (int) I;
 *
 *             which means no filter action.
 *
 *          Further, the operation of the stepper.step(STEPS);
 *          function call waits, moving the ballscrew, then returns.
 *
 * Note that the issue is bit chatter from the Arduino pin A0 A/D, (or
 * sensor,) which occurs at a maximum of ~ 10 kHz. rate, but due to
 * the stepper.step() call(s) waiting to move the ballscrew before
 * returning, occurs at a (50 * 200) / 60 = 166 steps per second rate,
 * (note that the sample rate is 166 Hz.):
 *
 *     A pole frequency of 166 Hz., (the single bit change sample
 *     rate,) would reduce the magnitude of the bit chatter errors by
 *     about -3 dB = 0.707:
 *
 *         k1 = exp (-2 * pi / 1)
 *               0.00186744273170798882
 *         k2 = 1 - k1
 *               0.99813255726829201118
 *
 *     And reduce the slew response, (to a bit change from the Arduino
 *     A0 A/D pin,) by about half, since it would take about 2 bits of
 *     successive change to pass through the filter.
 *
 *     A pole frequency of ten times the sample frequency, (166 Hz.,)
 *     or 1660 Hz., would reduce the magnitude of the bit chatter
 *     errors by about 1 dB, (actually by 0.981,) and have an
 *     insignificant effect on slew response.
 *
 *         k1 = exp (-2 * pi / 10)
 *         k1
 *              0.53348809109110325118
 *         k2 = 1 - k1
 *         k2
 *              0.46651190890889674882
 *
 * But notice that the call to stepper.step() in is blocking, (e.g.,
 * the call to stepper.step() does not return to loop() when the
 * stepper motor is slew rate limited, until the slew is finished.)
 * Obviously, there is no bit chatter in this scenario.
 *
 * However, when the stepper motor is static, (i.e., there is no
 * movement required,) the loop() function runs at about the speed of
 * the Arduino A/D A0 pin, ~ 10kHz. And, there is bit chatter in this
 * scenario.
 *
 * So, the objective is to design a filter that does not interfere
 * with the single bit stepping of the stepper motor, (i.e.,
 * incrementing the stepper motor by one step does so-that should not
 * be filtered,) and, the bit chatter from the Arduino A/D A0 pin
 * should be filtered:
 *
 *     1) With a sample frequency of (50 * 200) / 60 = 166 steps per
 *        second, (e.g., single stepping the stepper motor, as fast as
 *        it can be single stepped,) the filter should have no effect.
 *
 *     2) With a sample frequency of ~ 10 kHz., (e.g., the stepper
 *        motor is static, and the Arduino A/D A0 pin runs as fast as
 *        it can,) the bit chatter should be filtered.
 *
 *     The optimal pole frequency is then the geometric mean of these
 *     two frequencies:
 *
 *         sqrt (166 * 10000) = 1288.40987267251254946443 Hz.
 *
 *         k1 = exp (-2 * pi / (10000 / 1288.40987267251254946443))
 *         k1
 *              0.44506639835497673427
 *         k2 = 1 - k1
 *         k2
 *              0.55493360164502326573
 *
 *     Note that the pole frequency for the 10 kHz. is ~ 1/10 X the
 *     sample frequency, and, the pole pole for 166 Hz s ~ 10 X the
 *     sample frequency. The bit chatter for a static stepper motor
 *     will be reduced by ~ 20 dB, (~ 10 X,) and, the filter response
 *     for single stepping the the stepper motor will be down by ~
 *     0.05 dB, (~ 0.995 X.)
 *
 * Empirical measurements, (via enabling the PRINTIT #define, and
 * processing the output file,) indicates that lowering the pole
 * frequency to about 1 kHz. (about 20%,) gives slightly better
 * results than the optimal solution.
 *
 * The timing values used in the analysis should be verified; the
 * values were obtained from the BOM, (Bill of Materials,) data
 * sheets.
 *
 * The BOM empirically measured:
 *
 *    OMC/Stepperonline NEMA 17 stepper motor
 *    Adafruit TB6612 stepper motor driver
 *    ELEGOO MEGA 2560 R3 Arduino board
 *    An 12V 5A LED light power supply for the stepper motor
 *    The 5V supply was from the PC USB system
 *
 * The preamble and setup () function:
 *
 *     #include <Stepper.h>
 *     #define STEPS 200
 *     Stepper stepper(STEPS, 4, 5, 6, 7);
 *
 *     int previous = 0;
 *     int ctr;
 *     int I;
 *
 *     void setup()
 *     {
 *        stepper.setSpeed (50);
 *        Serial.begin (9600);
 *        delay (2000); while (!Serial); //delay for Leonardo
 *        Serial.print ("Ready:\n");
 *     }
 *
 * The loop () function for the slew rate limited test:
 *
 *     void loop()
 *     {
 *       // Slew rate limited movement:
 *
 *       Serial.print ("Start\n");
 *
 *       Serial.print ("       Up\n");
 *       I = analogRead(0);
 *       I = 1000; // avoid compiler optimization issues
 *       stepper.step(I);
 *       Serial.print ("       Down\n");
 *       I = analogRead(0);
 *       I = -1000; // avoid compiler optimization issues
 *       stepper.step(I);
 *
 *       Serial.print ("Stop\n");
 *     }
 *
 * The output to the serial monitor was timed with a cooking timer,
 * and found to be 14,000 steps in 85 seconds, or, 164.7 steps per
 * second, (the value used in the analysis was 166 steps per second.)
 * Slew rate limited calls to the stepper.step () function do block
 * during slew, as expected.
 *
 * The loop () function for the single step test:
 *
 *     void loop()
 *     {
 *       // Single step movement:
 *
 *       Serial.print ("Start\n");
 *
 *       for (ctr = 0; ctr < 1000; ctr ++)
 *       {
 *           I = analogRead(0);
 *           I = 1; // avoid compiler optimization issues
 *           stepper.step(I);
 *       }
 *
 *       for (ctr = 0; ctr < 1000; ctr ++)
 *       {
 *           I = analogRead(0);
 *           I = -1; // avoid compiler optimization issues
 *           stepper.step(I);
 *       }
 *
 *       Serial.print ("Stop\n");
 *     }
 *
 * The output to the serial monitor was timed with a cooking timer,
 * and found to be 14,000 steps in 85 seconds, or, 164.7 steps per
 * second, (the value used in the analysis was 166 steps per second.)
 *
 * The loop () function for the static test:
 *
 *     void loop()
 *     {
 *       // Static, no movement:
 *
 *       Serial.print ("Start\n");
 *
 *       for (ctr = 0; ctr < 30000; ctr ++)
 *       {
 *           I = analogRead(0);
 *           I = 0; // avoid compiler optimization issues
 *           stepper.step(I);
 *       }
 *
 *       Serial.print ("Stop\n");
 *     }
 *
 * The output to the serial monitor was timed with a cooking timer,
 * and found to be 33,000 iterations in 34 seconds, or, 9705.9
 * iterations per second, (the value used in the analysis was 10,000
 * iterations per second.)
 *
 * Measurements:
 *
 * The Arduino analog input, pin A0, was connected to a 2.56 filtered
 * Voltage reference, and the PRINTIT #define enabled in the
 * sketch. The serial monitor output was redirected to a file, and
 * processed with the programs in the fractal.tar.gz tape archive,
 * available from http://www.johncon.com/ndustrix/, comparing the 'I'
 * and 'val' values. There were 10K samples in the file, (i.e.,
 * iterations of the loop () function):
 *
 *  tsderivative I | tsrms -p
 *  0.946898
 *
 *  Meaning the Arduino A/D had a 1 LSB noise level, RMS. (The Arduino
 *  A/D is 10 bits, 1023 levels, or the noise level is about 0.1%.)
 *
 *  tsderivative I | sort -n | tscount
 *  1       -4.000000
 *  25      -3.000000
 *  351     -2.000000
 *  3175    -1.000000
 *  4346    0.000000
 *  2925    1.000000
 *  366     2.000000
 *  96      3.000000
 *  3       4.000000
 *
 *  Which is the distribution of the 1 LSB RMS noise level in the
 *  Arduino A/D system. (Notice the 4 sigma = 4 LSB values; the
 *  distribution is fairly close to a Gaussian/Normal distribution.)
 *
 *  tsderivative val | tsrms -p
 *  0.145813
 *
 *  Meaning the single pole filtered Arduino A/D output had a 0.14 LSB
 *  noise level, RMS, a reduction by a factor of 0.15 = 16 dB.
 *
 *  tsderivative val | sort -n | tscount
 *  120     -1.000000
 *  11048   0.000000
 *  120     1.000000
 *
 *  Which is the distribution of the 0.14 LSB noise level of the
 *  single pole filtered Arduino A/D output.
 *
 *  There were 12259 iterations through the loop () function, (i.e.,
 *  A/D samples,) and 7913 had "bit chatter," i.e., the stepper motor
 *  would have moved 1 to 4 steps, erroneously.
 *
 *  This was reduced to 240 by the single pole filter, a reduction
 *  by a factor of 98%.
 *
 *  A note about the measurements: the testing was done with the
 *  Arduino IDE, on a PC, with the Arduino connected to the PC USB
 *  system, which supplied the 5 V to to the Arduino. PC ground
 *  systems are notoriously noisy, with many spurious responses due to
 *  the high switching currents and switching power supplies. The
 *  Arduino was operating in a noisy environment, but still managed to
 *  acquit itself well for an inexpensive A/D. The environment was
 *  "realistic" for many Arduino applications.
 *
 */

#include <Stepper.h>

// Uncomment to print intermediate values to the serial
// monitor:
// #define PRINTIT

// Set the filter parameters for the single pole filter, (change the
// type from double to float if desired,) choice of either:
//
// Optimal, 1.288 kHz.
// double k1 = 0.44506639835497673427;
// double k2 = 0.55493360164502326573;
//
// Empirically determined superior, 1.000 kHz.:
double k1 = 0.53348809109110325118;
double k2 = 0.46651190890889674882;

// change this to the number of steps on your motor
#define STEPS 200

// create an instance of the stepper class, specifying
// the number of steps of the motor and the pins it's
// attached to; adafruit pins
Stepper stepper(STEPS, 4, 5, 6, 7);

// the previous reading from the analog input
int previous = 0;

void setup() {
  // set the speed of the motor to 50 RPMs, maximum
  stepper.setSpeed(50);

#ifdef PRINTIT

  Serial.begin (9600);
  delay (2000); while (!Serial); //delay for Leonardo
  Serial.print ("Ready:\n");

#endif

}

void loop() {
  // get the sensor value; 0 to 5 V DC, filtered
  int I = analogRead(0);
  // integer cast always rounds down
  int val = (int) ((((double) I * k2) + ((double) previous * k1)) + (double) 0.5);

#ifdef PRINTIT

  Serial.print("I = ");
  Serial.print(I, DEC);
  Serial.print("; val = ");
  Serial.print(val, DEC);
  Serial.print("; previous = ");
  Serial.print(previous,DEC);
  Serial.print("; val - previous = ");
  Serial.print(val - previous, DEC);
  Serial.print("\n");

#endif

  // move a number of steps equal to the change in the
  // filtered sensor reading
  stepper.step(val - previous);

  // remember the previous value of the sensor
  previous = val;
}