//
// now = current time in microseconds
// lastTime = time at last step in microseconds
// timeChange = elasped time since last step in microseconds (delta t)
// tau = filtering time constant in microseconds (must be expressed in same units as (delta t))
// Setpoint = desired position (expressed as potentiometer value)
// Input = current position (expressed as potentiometer value)
// Error = position error
// Last = position error at previous step
// vf = velocity filtered once {units = counts/s}
// vff = velocity filtered a second time {units = counts/s}
//
// calculate (delta t) and error
//
  now = micros();
  timeChange = now-lastTime;
  Error = Setpoint-Input;
//  Error = Input;
  Integral = Integral + Error;
//
  df = (timeChange / (tau + timeChange))*Error + (tau / (tau + timeChange))*df;
//
// calculate filtered velocity
//
// velocity = (Error-Last) / (timeChange/1000000) {units = counts/s}
//
//
  vf = (timeChange / (tau + timeChange)) * (Error-Last) / (timeChange/1000000)
     + (tau / (tau + timeChange))*vf;
//
// filter velocity a second time
//
  vff = (timeChange / (tau + timeChange))*vf + (tau / (tau + timeChange))*vff;
//
.
.
.
.      Other PID commands can be here
.
.
.
.
//
// save time and error to be used in the next step
//
  lastTime = now;
  Last = Error;