sampling/avg/avg.ino → hello-world/sonic-HC-SR04/sonic-HC-SR04.ino

@@ -1,5 +1,8 @@
+#include "src/sensor_ema/sensor_ema.h"
 #include <LiquidCrystal_I2C.h>
 
+const char * sensorName = "SR04";
+const int dp = 2;
 /*
  * Arduino UNO:
  *
@@ -11,28 +14,26 @@
  */
 LiquidCrystal_I2C lcd(0x27, 16, 2);
 
-double ema = 0;
 
-/*
- * Timing vars (milliseconds)
- */
+// HC-SR04 Pins
+const int trigPin = 3;
+const int echoPin = 2;
 unsigned long lastRead = 0;
-unsigned long lastWrite = 0;
-const unsigned long interval = 2000;
-
-// If no sensor, can use a PRNG
-bool pseudoRandomNumberGenerator = true;
-const int seed = 0;
 
-// EMA vars
-double alpha = 0.1;
+void my_init() {
+  lcd.init();
+  //lcd.begin(16, 2);
+  lcd.backlight();
+  lcd.clear();
 
-const char * sensorName = "SR04";
-const int dp = pseudoRandomNumberGenerator ? 0 : 2;
+  // Initialize HC-SR04 pins
+  pinMode(trigPin, OUTPUT);
+  pinMode(echoPin, INPUT);
 
-// HC-SR04 Pins
-const int trigPin = 3;
-const int echoPin = 2;
+  lcd.print("Sampling ");
+  lcd.print(sensorName);
+  lcd.print("...");
+}
 
 // Function to read distance in cm
 float readDistance() {
@@ -66,68 +67,27 @@ float readDistance() {
   return distance;
 }
 
-int myRead(){
-  return readDistance();
-}
-
-int read(){
-  if (pseudoRandomNumberGenerator)
-  { 
-    return random(0, 1024);
-  }
-  else
-  {
-    return myRead();
-  }
+void my_write(float val) {
+    lcd.setCursor(0, 1);
+    lcd.print("EMA: ");
+    lcd.print(val, dp);
+    lcd.print("   ");
 }
 
-void display(){
-  lcd.print("EMA: ");
-  lcd.print(ema, dp);
-  lcd.print("   ");
-}
+// --- Global Context ---
+SensorEMA_t sensorCtx;
 
 void setup() {
-  lcd.init();
-  lcd.begin(16, 2);
-  lcd.backlight();
-  lcd.clear();
-
-  // Initialize HC-SR04 pins
-  pinMode(trigPin, OUTPUT);
-  pinMode(echoPin, INPUT);
+    // Assign function pointers
+    sensorCtx.fn_init = my_init;
+    sensorCtx.fn_read = readDistance;
+    sensorCtx.fn_write = my_write;
 
-  if (pseudoRandomNumberGenerator)
-  { 
-    randomSeed(seed);
-  }
-  ema = read();
-  
-  lcd.setCursor(0, 0);
-  if (pseudoRandomNumberGenerator)
-  {
-    lcd.print("Sampling PRNG...");
-  }
-  else
-  {
-    lcd.print("Sampling ");
-    lcd.print(sensorName);
-    lcd.print("...");
-  }
+    // Initialize Library (Alpha=0.1, Interval=2000ms)
+    SensorEMA_setup(&sensorCtx, 0.1, 2000);
 }
 
 void loop() {
-  long curMillis = millis();
-  int raw = read();
-
-  if (raw >= 0){
-    ema = (raw * alpha) + (ema * (1.0 - alpha));
-  }
-
-  if (curMillis - lastWrite >= interval)
-  {
-    lastWrite = curMillis;
-    lcd.setCursor(0, 1);
-    display();
-  }
+    // Call library loop as fast as possible
+    SensorEMA_loop(&sensorCtx, millis());
 }

hello-world/sonic-HC-SR04/src/sensor_ema

@@ -0,0 +1 @@
+../../../lib/sensor_ema

lib/sensor_ema/sensor_ema.cpp

@@ -0,0 +1,43 @@
+#include "sensor_ema.h"
+
+void SensorEMA_setup(SensorEMA_t* ctx, float alpha, unsigned long interval_ms) {
+    ctx->alpha = alpha;
+    ctx->update_interval_ms = interval_ms;
+    ctx->current_ema = 0.0;
+    ctx->last_update_time = 0;
+    ctx->has_valid_data = false;
+
+    // Run user init if provided
+    if (ctx->fn_init) {
+        ctx->fn_init();
+    }
+
+    // Seed EMA with first reading if possible
+    if (ctx->fn_read) {
+        float val = ctx->fn_read();
+        if (val >= 0) {
+            ctx->current_ema = val;
+            ctx->has_valid_data = true;
+        }
+    }
+}
+
+void SensorEMA_loop(SensorEMA_t* ctx, unsigned long  now) {
+    if (!ctx->fn_read || !ctx->fn_write) return;
+
+    float raw = ctx->fn_read();
+
+    if (raw >= 0.0) {
+        if (!ctx->has_valid_data) {
+            ctx->current_ema = raw;
+            ctx->has_valid_data = true;
+        } else {
+            ctx->current_ema = (raw * ctx->alpha) + (ctx->current_ema * (1.0 - ctx->alpha));
+        }
+    }
+
+    if (now - ctx->last_update_time >= ctx->update_interval_ms) {
+        ctx->last_update_time = now;
+        ctx->fn_write(ctx->current_ema);
+    }
+}

lib/sensor_ema/sensor_ema.h

@@ -0,0 +1,30 @@
+#ifndef SENSOR_EMA_H
+#define SENSOR_EMA_H
+
+#include <stdint.h>
+#include <stdbool.h>
+
+typedef void (*ema_init_func_t)(void);
+typedef float (*ema_read_func_t)(void);
+typedef void (*ema_write_func_t)(float val);
+
+typedef struct {
+    float alpha;
+    unsigned long update_interval_ms;
+
+    float current_ema;
+    unsigned long last_update_time;
+    bool has_valid_data;
+
+    ema_init_func_t fn_init;
+    ema_read_func_t fn_read;
+    ema_write_func_t fn_write;
+
+} SensorEMA_t;
+
+// XXX: Should we pass function pointers in?
+void SensorEMA_setup(SensorEMA_t* ctx, float alpha, unsigned long interval_ms);
+
+void SensorEMA_loop(SensorEMA_t* ctx, unsigned long now);
+
+#endif