Cruise Control - Teetor's Speedostat

// A speed regulator with a load disturbance you apply with your thumb.
//
// This is the cruise-control problem in miniature and it is NOT the same problem as the
// heater loop. There the job was to reach a setpoint; here the setpoint never moves and
// the whole job is rejecting a disturbance -- a hill, a headwind, a trailer, or in this
// case a finger on the flywheel.
//
// Hardware: a geared DC motor with a slotted disc on the shaft, an optical detector
// straddling the disc, and a logic-level MOSFET driving the motor from PWM. Put a rubber
// pad on a lever where you can press it against the disc rim: that is the hill.
//
// Set MODE = 1 for proportional only and watch the speed sag whenever you press. Set
// MODE = 2 and the sag disappears -- slowly, because the integral has to accumulate.

const int PIN_MOTOR = 9;       // PWM to the MOSFET gate
const int PIN_TACHO = 2;       // optical detector, interrupt-capable
const int SLOTS     = 20;      // slots in the disc

// 1 = proportional only (a 1950s vacuum servo)   2 = proportional + integral
const int MODE = 1;

const float SETPOINT = 60.0;   // rev/min
const float KP       = 1.8;    // PWM counts per rev/min of error
const float KI       = 0.6;    // PWM counts per rev/min per second
const float DT       = 0.10;   // seconds per loop
const float OUT_MAX  = 255.0;
const float OUT_MIN  = 0.0;

volatile unsigned long pulses = 0;
float integral = 0.0;
unsigned long tPrev = 0;

void countPulse() { pulses++; }

void setup() {
  pinMode(PIN_MOTOR, OUTPUT);
  pinMode(PIN_TACHO, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(PIN_TACHO), countPulse, RISING);
  Serial.begin(115200);
  Serial.println(F("t_s,rpm,output,integral"));
  tPrev = millis();
}

void loop() {
  if (millis() - tPrev < (unsigned long)(DT * 1000)) return;
  tPrev = millis();

  noInterrupts();
  unsigned long n = pulses;
  pulses = 0;
  interrupts();

  float rpm   = (n / (float)SLOTS) * (60.0 / DT);
  float error = SETPOINT - rpm;

  float raw = KP * error;
  if (MODE == 2) raw += KI * integral;

  float out = constrain(raw, OUT_MIN, OUT_MAX);
  // Anti-windup again: hold the integral still whenever the motor is flat out, or a hard
  // press followed by a release sends the disc to full speed while the term unwinds.
  if (MODE == 2 && raw == out) integral += error * DT;

  analogWrite(PIN_MOTOR, (int)out);

  Serial.print(millis()/1000.0, 1); Serial.print(',');
  Serial.print(rpm, 1);             Serial.print(',');
  Serial.print(out, 0);             Serial.print(',');
  Serial.println(integral, 1);
}