Final Pulsemeter

pulsemeter_v3.ino

#define NAVG 128

#define SENSOR_INPUT A3
#define SENSOR_INPUT2 A1

#define NORMAL_BUTTON 13
#define FILTER_BUTTON 2
#define PULSE_BUTTON 3

#define RED_LED 9
#define YELLOW_LED 8
#define GREEN_LED 6

int xs[NAVG];
long ds[NAVG];
int curAvg, curDev;
int mode;
int greenLedOff = 0;

void setup() {
  for (int i = 0; i < NAVG; i++) {
    xs[i] = 0;
    ds[i] = 0;
  }
  mode = NORMAL_BUTTON;
  pinMode(SENSOR_INPUT, INPUT);
  pinMode(SENSOR_INPUT2, INPUT);
  pinMode(NORMAL_BUTTON, INPUT_PULLUP);
  pinMode(FILTER_BUTTON, INPUT_PULLUP);
  pinMode(PULSE_BUTTON, INPUT_PULLUP);
  pinMode(RED_LED, OUTPUT);
  pinMode(YELLOW_LED, OUTPUT);
  pinMode(GREEN_LED, OUTPUT);
  Serial.begin(115200);
}

int filter(int x) {
    // f0 = 2, bw = 2
    //static int a0 = 1024, a1 = -1857, a2 = 846;
    //static int b0 = 88, b1 = 0, b2 = -88;
    // f0 = 1, bw = 1
    //static int a0 = 1024, a1 = -2000, a2 = 979;
    //static int b0 = 22, b1 = 0, b2 = -22;
    static int a0 = 1024, a1 = -1940, a2 = 924;
    static int b0 = 50, b2 = -50;
    static int xp = 0, xpp = 0;
    static int yp = 0, ypp = 0;
    int y = (int) ((b0 * (long) x + b2 * (long) xpp
            - a1 * (long) yp - a2 * (long) ypp) / a0);
    ypp = yp;
    yp = y;
    xpp = xp;
    xp = x;
    return y;
}

void average(int x) {
  static int iAvg = 0;
  static long totAvg = 0;
  static long totDev = 0;
  totAvg += x - xs[iAvg];
  xs[iAvg] = x;
  curAvg = (int) (totAvg / NAVG);
  long dev = x - curAvg;
  dev *= dev;
  totDev += dev - ds[iAvg];
  ds[iAvg] = dev;
  curDev = (int) sqrt(totDev / NAVG);
  iAvg += 1;
  if (iAvg >= NAVG) {
    iAvg = 0;
  }
}

long measure(int count) {
  long res = 0;
  for (int i = 0; i < count; i += 1) {
    res += analogRead(SENSOR_INPUT);
  }
  return res;
}

void reportBeat(double p) {
  static long t0 = 0;
  static double pAvg = 0;
  if (mode == -PULSE_BUTTON) {
    mode = PULSE_BUTTON;
    t0 = millis();
    pAvg = p;
  } else {
    pAvg += (p - pAvg) / 5;
  }
  if (mode == PULSE_BUTTON) {
    Serial.print((millis() - t0)/1000.0);
    Serial.print(' ');
    Serial.println(p);
  }
  pinMode(GREEN_LED, INPUT);
  greenLedOff = 10;
}

void pulseDetected() {
  static long t = 0;
  long t1 = millis();
  int dt = (int) (t1 - t);
  reportBeat(60000.0 / dt);
  t = t1;
}

void showLeds(int m1000) {
  if (greenLedOff > 0) {
    greenLedOff -= 1;
    if (greenLedOff == 0) {
      pinMode(GREEN_LED, OUTPUT);
    }
  }
  digitalWrite(GREEN_LED, m1000 > 350 && m1000 <= 850 ? HIGH : LOW);
  digitalWrite(RED_LED, m1000 > 850 ? HIGH : LOW);
  digitalWrite(YELLOW_LED, m1000 < 350 ? HIGH : LOW);
}

void detectButtons() {
  if (digitalRead(NORMAL_BUTTON) == LOW) {
    mode = NORMAL_BUTTON;
  } else if (digitalRead(FILTER_BUTTON) == LOW) {
    mode = FILTER_BUTTON;
  } else if (digitalRead(PULSE_BUTTON) == LOW) {
    if (mode != PULSE_BUTTON) {
      mode = -PULSE_BUTTON;
    }
  }
}

void loop() {
  delay(1);
  int m = measure(16);
  showLeds(m / 16);
  detectButtons();
  static int fp = 0;
  int f = filter(m);
  static boolean waiting = false;
  static long t0 = 0;
  static double pAvg = 0;
  average(f);
  if (f > curAvg) {
    waiting = true;
  }
  if (f < curAvg - curDev / 2 && f > fp && waiting) {
    waiting = false;
    pulseDetected();
  }
  fp = f;
  if (mode == FILTER_BUTTON) {
    Serial.print(f);
    Serial.print(' ');
    Serial.print(curAvg);
    Serial.print(' ');
    Serial.println(curDev);
  } else if (mode == NORMAL_BUTTON) {
    Serial.println(m);
  }
  delay(8);
}