The Voice-Coil Actuator and Track Following

// A real track-following servo, at a scale you can see.
//
// A voice coil is a force source with no gearing and no stiction, which is why every hard
// disk uses one and why a loudspeaker motor is the same part. Drive one from a position
// error and you have the actuator of blueprint 6.
//
// Hardware:
//   - a loudspeaker (any size; a 3-inch full-range is ideal) with a light pointer glued
//     to the cone
//   - an IR reflective sensor or a slot sensor watching the pointer -> GPIO34 (position)
//   - an H-bridge or a single power op-amp driving the voice coil from GPIO25 (DAC)
//   - do NOT drive the coil straight from a GPIO; a voice coil is 4-8 ohms
//
// What to look for: raise KD to zero and the loop will oscillate no matter how small KP
// is. That is not a tuning failure, it is the double integrator -- position lags force by
// 180 degrees all on its own, so proportional control alone has no phase margin at all.

const int PIN_POS = 34;      // position sensor
const int PIN_DRV = 25;      // DAC -> power stage

// --- gains. Start here, then follow the experiment in the step text.
float KP = 0.60;
float KI = 0.08;
float KD = 0.22;             // set this to 0.0 and watch it ring

const float DT = 0.002;      // 500 Hz loop
float integ = 0.0, prevErr = 0.0;

int   target = 2048;         // the "track" we are following
long  tStep  = 0;

float readPos() {
  long acc = 0;
  for (int i = 0; i < 4; i++) acc += analogRead(PIN_POS);
  return acc / 4.0;
}

void drive(float u) {
  // u is signed; the power stage takes 0..255 with 128 as zero force
  int v = (int)(128 + u);
  if (v < 0) v = 0;
  if (v > 255) v = 255;
  dacWrite(PIN_DRV, v);
}

void setup() {
  Serial.begin(115200);
  delay(300);
  analogReadResolution(12);
  analogSetPinAttenuation(PIN_POS, ADC_11db);
  drive(0);
  delay(500);

  Serial.println("# voice-coil track following");
  Serial.printf("# KP=%.2f KI=%.2f KD=%.2f, loop %.0f Hz\n", KP, KI, KD, 1.0/DT);
  Serial.println("ms,target,position,error,drive");
}

void loop() {
  static unsigned long last = 0;
  unsigned long now = micros();
  if (now - last < (unsigned long)(DT * 1e6)) return;
  last = now;

  // Step the target every 500 ms: this is a one-track seek.
  if (++tStep % 250 == 0) target = (target == 2048) ? 2248 : 2048;

  float pos = readPos();
  float err = target - pos;

  integ += err * DT;
  if (integ >  4000) integ =  4000;        // anti-windup, or a saturated stage never recovers
  if (integ < -4000) integ = -4000;

  float deriv = (err - prevErr) / DT;
  prevErr = err;

  float u = KP * err + KI * integ + KD * deriv;
  if (u >  127) u =  127;
  if (u < -127) u = -127;
  drive(u);

  Serial.printf("%lu,%d,%.0f,%.0f,%.1f\n", millis(), target, pos, err, u);
}