1-RAK-8-Peripherals

1-RAK-8-Peripherals.ino

/*
  Author: Johann Suarez (VE7-IPC) 
  Date: October 1, 2025
  Description: 
    Offloading peripheral handling to an ESP32-Microcontroller
    by communicating with a Meshtastic node via UART.
    
    
    
  October 2, 2025
  The Meshtastic library that you'll need in the Arduino IDE is outdated.
  Its latest release is 0.0.7 (December 2024) but this code uses functions 
  they added to the library more recently (May 2025).

  Simply download the library repository as a .zip from here:
  https://github.com/meshtastic/Meshtastic-arduino

  then manually add it as a library from Arduino IDE.

  The library is needed if you are communicating serially in
  Protobuf mode (recommended). This has advantages
  over the "SIMPLE" serial mode:
      - You can be more selective about which channels/users to send to,
          instead of being stuck listening/receiving in one channel.
      - You can check the node number of the sender for security (instead of trusting on a plaintext username).
      - You can check whether a message is a broadcast, or is sent directly to you.

*/

// Uncomment the line below to enable debugging 
// #define MT_DEBUGGING

#include <Meshtastic.h>

#include "Adafruit_seesaw.h"
// FreeRTOS headers are available under Arduino-ESP32
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"

// OLED Display Libraries
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

// Servo motor library
#include <ESP32Servo.h>

// Pins to use for WiFi; these defaults are for an Adafruit Feather M0 WiFi.
#define WIFI_CS_PIN 8
#define WIFI_IRQ_PIN 7
#define WIFI_RESET_PIN 4
#define WIFI_ENABLE_PIN 2

// Pins to use for SoftwareSerial. Boards that don't use SoftwareSerial, and
// instead provide their own Serial1 connection through fixed pins
// will ignore these settings and use their own.
#define SERIAL_RX_PIN 5
#define SERIAL_TX_PIN 4
// A different baud rate to communicate with the Meshtastic device can be specified here
#define BAUD_RATE 57600

// ---- Buzzer ----
#define BUZZER_PIN 20

// ---- LED Light ----
#define LED_PIN 21

// ---- MH-Z19B (PWM) on GPIO 6 ----
#define CO2_PWM_PIN 6
#define CO2_TIMEOUT_MS 2500   // consider data stale if no full PWM period seen within this

// ---- Servo SG90 ----
#define SERVO_PIN 10

// ---- AC Relay ----
#define RELAY_PIN 7
#define RELAY_ACTIVE_LOW true 

// Custom I2C Pins
#define I2C_SDA_PIN 3
#define I2C_SCL_PIN 1
#define I2C_HZ      400000

// ---- OLED setup ----
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 32
#define OLED_RESET    -1
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

// ---- Servo ----
Servo g_servo;
/*
  The design pattern for peripheral handling:
  
  Each peripheral regardless of communication protocol is assigned a struct
  with variables for its housekeeping. Then a function is dedicated to it
  for performing the actual housekeeping. 
*/

struct {
  bool active;
  uint32_t until_ms;
} g_buzzer = {false, 0};


struct {
  bool active;
  uint32_t until_ms;
} led_light = {false, 0};


// ---- One mutex to own the I2C bus (sensors + OLED) ----
SemaphoreHandle_t g_i2c_mutex = nullptr;

// ---- OLED message state ----
struct {
  bool active;
  uint32_t until_ms;
  bool drawn;
} g_oled_msg = {false, 0, false};

// ---- servo state ----
struct {
  bool active;
  uint32_t until_ms;
  int target_deg;
  int default_deg;
} g_servo_state = {false, 0, 30, 30};  // default to 30°

// ---- AC relay ----
struct {
  bool on;
} g_relay = { true };

// Send a text message every this many seconds
#define SEND_PERIOD 60

uint32_t next_send_time = 0;
bool not_yet_connected = true;

// Change this to a specific node number if sending to just one node
uint32_t dest = BROADCAST_ADDR; 
// Change this to another index if sending on a different channel
uint8_t channel_index = 1; 

// Global cache, volatile for safe cross-task reads
volatile uint32_t plant_sensor_1 = 0;  // updated by SensorTask
volatile uint32_t plant_sensor_2 = 0;
volatile uint32_t MLX_temp_sensor = (uint32_t)-1;

// public cache (ppm) for MH-Z19B module, updated by service
volatile uint32_t co2_ppm = (uint32_t)-1;  

//---------------=| Sensor Concerns |=----------------------------------//
enum PS_State : uint8_t { PS_INIT, PS_ONLINE, PS_ERROR };

// Seesaw (I2C)
struct PlantSensor {
  // Config
  uint8_t  addr;             // e.g., 0x36, 0x37
  uint8_t  channel;          // seesaw channel, usually 0
  uint32_t period_ms;        // sampling cadence per sensor
  uint32_t phase_ms;         // start offset to avoid clumps
  // Driver + state
  Adafruit_seesaw ss;
  PS_State state = PS_INIT;
  uint8_t fail_count = 0;
  uint32_t next_due_ms = 0;  // next allowed time to act
  // Filter: ring buffer (trimmed mean) — use EMA if you prefer
  static constexpr size_t WIN = 10;
  uint16_t ring[WIN] = {0};
  size_t idx = 0, filled = 0;
  // Output & health
  uint32_t filtered = 0;
  bool valid = false;
};

static PlantSensor g_sensors[] = {
  { /*addr*/0x38, /*ch*/0, /*period*/1000, /*phase*/0   },  // Sensor A
  { /*addr*/0x36, /*ch*/0, /*period*/1000, /*phase*/500 },  // Sensor B (staggered by 0.5 s)
};

// Map sensor i → public cache pointer (so you can publish N sensors easily)
static volatile uint32_t* g_public_cache[] = {
  &plant_sensor_1,
  &plant_sensor_2,
};

struct MHZ19_PWM {
  uint8_t pin;
  // ISR-updated timings (microseconds)
  volatile uint32_t last_rise_us = 0;
  volatile uint32_t last_fall_us = 0;
  volatile uint32_t high_us = 0;
  volatile uint32_t low_us = 0;
  // housekeeping
  uint32_t last_period_ms = 0;   // when we last observed a full period
  bool     valid = false;        // have we seen at least one good period?
} g_co2 = { CO2_PWM_PIN };



// ---------- Small helpers ----------
static inline uint32_t now_ms() { return (uint32_t) (xTaskGetTickCount() * portTICK_PERIOD_MS); }

static uint32_t trimmed_mean_u16(const uint16_t* buf, size_t count) {
  if (!count) return 0;
  uint32_t sum = 0; uint16_t mn = UINT16_MAX, mx = 0;
  for (size_t i = 0; i < count; ++i) { uint16_t v = buf[i]; sum += v; if (v < mn) mn = v; if (v > mx) mx = v; }
  if (count >= 3) return (sum - mn - mx) / (uint32_t)(count - 2);
  return sum / (uint32_t)count;
}

static void ps_set_backoff(PlantSensor& ps) {
  // exponential backoff capped at 8s
  uint32_t backoff = 1000u << (ps.fail_count > 3 ? 3 : ps.fail_count); // 1,2,4,8s
  ps.next_due_ms = now_ms() + backoff;
}

static bool ps_try_init(PlantSensor& ps) {
  bool ok = false;
  if (xSemaphoreTake(g_i2c_mutex, portMAX_DELAY)) {
    ok = ps.ss.begin(ps.addr);
    xSemaphoreGive(g_i2c_mutex);
  }
  if (ok) {
    ps.state = PS_ONLINE;
    ps.fail_count = 0;
    ps.next_due_ms = now_ms() + ps.phase_ms;
    return true;
  }
  ps.state = PS_ERROR;
  ps.fail_count++;
  ps_set_backoff(ps);
  return false;
}

// ---------- The sensor task: one action per wake ----------
static void SensorTask(void* pv) {
  (void)pv;
  //Wire.begin(I2C_SDA_PIN, I2C_SCL_PIN);

  // Prime INIT state tries
  for (size_t i = 0; i < sizeof(g_sensors)/sizeof(g_sensors[0]); ++i) {
    g_sensors[i].state = PS_INIT;
  }

  const TickType_t tick = pdMS_TO_TICKS(25);  // wake ~40×/s to look for due work
  for (;;) {
    uint32_t now = now_ms();
    bool did_work = false;

    for (size_t i = 0; i < sizeof(g_sensors)/sizeof(g_sensors[0]); ++i) {
      PlantSensor& ps = g_sensors[i];
      if (now < ps.next_due_ms) continue;  // not due

      switch (ps.state) {
        case PS_INIT:
          ps_try_init(ps);
          did_work = true;
          break;

        case PS_ONLINE: {
          // Do exactly one sensor’s read this tick to limit bus load
          bool ok = ps_do_sample(ps);
          if (!ok) {  // (kept for symmetry; touchRead doesn’t expose errors cleanly)
            ps.state = PS_ERROR; ps.fail_count++; ps_set_backoff(ps);
          } else {
            // publish to public cache (single 32-bit write)
            *g_public_cache[i] = ps.filtered;
          }
          did_work = true;
        } break;

        case PS_ERROR:
          // time to retry init?
          ps_try_init(ps);
          did_work = true;
          break;
      }

      if (did_work) break;  // one action per wake; others wait till next 25ms tick
    }

    vTaskDelay(did_work ? pdMS_TO_TICKS(5) : tick);
  }
}

void startSensorTask() {
  xTaskCreate(SensorTask, "SensorTask", 4096, nullptr, 2, nullptr);
}


static bool ps_do_sample(PlantSensor& ps) {
  uint16_t raw = 0;
  if (xSemaphoreTake(g_i2c_mutex, portMAX_DELAY)) {
    raw = ps.ss.touchRead(ps.channel);
    xSemaphoreGive(g_i2c_mutex);
  } else {
    return false; // couldn't get bus (rare), try next tick
  }

  ps.ring[ps.idx] = raw;
  ps.idx = (ps.idx + 1) % PlantSensor::WIN;
  if (ps.filled < PlantSensor::WIN) ps.filled++;
  ps.filtered = trimmed_mean_u16(ps.ring, ps.filled);
  ps.valid = true;
  ps.next_due_ms += ps.period_ms;
  return true;
}


//---------------=| JSON Formatting Functions |=------------------------//

// Minimal JSON string escaper: handles \, ", and common control chars.
static void escape_json(const char* in, char* out, size_t out_size) {
  size_t o = 0;
  for (size_t i = 0; in[i] != '\0' && o + 1 < out_size; ++i) {
    char c = in[i];
    const char* esc = nullptr;
    switch (c) {
      case '\"': esc = "\\\""; break;
      case '\\': esc = "\\\\"; break;
      case '\n': esc = "\\n";  break;
      case '\r': esc = "\\r";  break;
      case '\t': esc = "\\t";  break;
      default:   esc = nullptr; break;
    }
    if (esc) {
      // write two chars if there's room
      if (o + 2 < out_size) {
        out[o++] = esc[0];
        out[o++] = esc[1];
      } else {
        break;
      }
    } else if ((unsigned char)c < 0x20) {
      // Generic control char -> skip or encode as space to keep it simple
      if (o + 1 < out_size) out[o++] = ' ';
    } else {
      out[o++] = c;
    }
  }
  out[o] = '\0';
}


// Returns pointer to a static buffer holding the JSON.
// Safe for simple usage within a single print; not re-entrant.
static const char* format_rx_json(uint32_t from, uint32_t to, uint8_t channel, const char* text) {
  static char json_buf[384];
  char esc_text[256];
  escape_json(text ? text : "", esc_text, sizeof(esc_text));

  // {"channel":<u8>,"from":<u32>,"to":<u32>,"text":"..."}
  snprintf(json_buf, sizeof(json_buf),
           "{\"channel\":%u,\"from\":%lu,\"to\":%lu,\"text\":\"%s\"}",
           (unsigned)channel,
           (unsigned long)from,
           (unsigned long)to,
           esc_text);
  return json_buf;
}

//---------------=| End of JSON Functions |=----------------------------//


//---------------=| Meshtastic Functions |=-----------------------------//

// This callback function will be called whenever the radio connects to a node
void connected_callback(mt_node_t *node, mt_nr_progress_t progress) {
  if (not_yet_connected) 
    Serial.println("Connected to Meshtastic device!");
  not_yet_connected = false;
}


const char* meshtastic_portnum_to_string(meshtastic_PortNum port) {
  switch (port) {
      case meshtastic_PortNum_UNKNOWN_APP: return "UNKNOWN_APP";
      case meshtastic_PortNum_TEXT_MESSAGE_APP: return "TEXT_MESSAGE_APP";
      case meshtastic_PortNum_REMOTE_HARDWARE_APP: return "REMOTE_HARDWARE_APP";
      case meshtastic_PortNum_POSITION_APP: return "POSITION_APP";
      case meshtastic_PortNum_NODEINFO_APP: return "NODEINFO_APP";
      case meshtastic_PortNum_ROUTING_APP: return "ROUTING_APP";
      case meshtastic_PortNum_ADMIN_APP: return "ADMIN_APP";
      case meshtastic_PortNum_TEXT_MESSAGE_COMPRESSED_APP: return "TEXT_MESSAGE_COMPRESSED_APP";
      case meshtastic_PortNum_WAYPOINT_APP: return "WAYPOINT_APP";
      case meshtastic_PortNum_AUDIO_APP: return "AUDIO_APP";
      case meshtastic_PortNum_DETECTION_SENSOR_APP: return "DETECTION_SENSOR_APP";
      case meshtastic_PortNum_REPLY_APP: return "REPLY_APP";
      case meshtastic_PortNum_IP_TUNNEL_APP: return "IP_TUNNEL_APP";
      case meshtastic_PortNum_PAXCOUNTER_APP: return "PAXCOUNTER_APP";
      case meshtastic_PortNum_SERIAL_APP: return "SERIAL_APP";
      case meshtastic_PortNum_STORE_FORWARD_APP: return "STORE_FORWARD_APP";
      case meshtastic_PortNum_RANGE_TEST_APP: return "RANGE_TEST_APP";
      case meshtastic_PortNum_TELEMETRY_APP: return "TELEMETRY_APP";
      case meshtastic_PortNum_ZPS_APP: return "ZPS_APP";
      case meshtastic_PortNum_SIMULATOR_APP: return "SIMULATOR_APP";
      case meshtastic_PortNum_TRACEROUTE_APP: return "TRACEROUTE_APP";
      case meshtastic_PortNum_NEIGHBORINFO_APP: return "NEIGHBORINFO_APP";
      case meshtastic_PortNum_ATAK_PLUGIN: return "ATAK_PLUGIN";
      case meshtastic_PortNum_MAP_REPORT_APP: return "MAP_REPORT_APP";
      case meshtastic_PortNum_POWERSTRESS_APP: return "POWERSTRESS_APP";
      case meshtastic_PortNum_PRIVATE_APP: return "PRIVATE_APP";
      case meshtastic_PortNum_ATAK_FORWARDER: return "ATAK_FORWARDER";
      case meshtastic_PortNum_MAX: return "MAX";
      default: return "UNKNOWN_PORTNUM";
  }
}

void displayPubKey(meshtastic_MeshPacket_public_key_t pubKey, char *hex_str) {
      for (int i = 0; i < 32; i++) {
          sprintf(&hex_str[i * 2], "%02x", (unsigned char)pubKey.bytes[i]);
      }
      hex_str[64] = '\0'; // Null terminator
}


void encrypted_callback(uint32_t from, uint32_t to,  uint8_t channel, meshtastic_MeshPacket_public_key_t pubKey, meshtastic_MeshPacket_encrypted_t *enc_payload) {
  Serial.print("Received an ENCRYPTED callback from: ");
  Serial.print(from);
  Serial.print(" to: ");
  Serial.println(to);
}

void portnum_callback(uint32_t from, uint32_t to,  uint8_t channel, meshtastic_PortNum portNum, meshtastic_Data_payload_t *payload) {
  Serial.print("Received a callback for PortNum ");
  Serial.println(meshtastic_portnum_to_string(portNum));
}

void command_parser(uint32_t from, uint32_t to,  uint8_t channel, const char* text) {


  if (to == my_node_num) {
    if (strcmp(text, "query_sensor") == 0) {
      Serial.println("Sensor Query command recognized.");
      char msg[64];  // temp buffer
      snprintf(msg, sizeof(msg), "Sensor 1 Val: %u, Sensor 2 Val: %u", plant_sensor_1, plant_sensor_2);
      mt_send_text(msg, from, channel_index);

      return;
    }
    else if (strcmp(text, "light_on") == 0) {
      Serial.println("Light On command recognized.");
      return;
    }
    else if (strcmp(text, "oled") == 0) {
      Serial.println("OLED command recognized.");
      g_oled_msg.active = true;
      g_oled_msg.drawn = false;                 // force redraw on next service
      g_oled_msg.until_ms = millis() + 5000;    // show for 5 seconds
      // (Optional) Acknowledge sender
      // mt_send_text("OLED: showing 'Hello' for 5s", from, channel_index);
      return;
    } else if (strcmp(text, "buzz") == 0) {
      Serial.println("Buzz command recognized.");
      g_buzzer.active = true;
      g_buzzer.until_ms = millis() + 5000;   // 5 seconds from now
      // (Optional) acknowledge sender
      // mt_send_text("Buzzing for 5 seconds", from, channel_index);
      return;
    }
    else if (strcmp(text, "led") == 0) {
      Serial.println("LED command recognized.");
      led_light.active = true;
      led_light.until_ms = millis() + 5000;   // 5 seconds from now
      // (Optional) acknowledge sender
      // mt_send_text("Buzzing for 5 seconds", from, channel_index);
      return;
    } else if (strcmp(text, "co2") == 0) {
      Serial.println("CO2 Query command recognized.");
      uint32_t ppm = co2_ppm;
      char msg[48];
      if (ppm == (uint32_t)-1) snprintf(msg, sizeof(msg), "CO2: stale");
      else                     snprintf(msg, sizeof(msg), "CO2: %lu ppm", (unsigned long)ppm);
      mt_send_text(msg, from, channel_index);
      return;
    } else if (strcmp(text, "servo") == 0) {
      Serial.println("Servo command recognized.");
      g_servo_state.active = true;
      g_servo_state.target_deg = 90;              // example: sweep to ~90°
      g_servo_state.until_ms = millis() + 5000;   // hold for 5 s
      // (Optional) mt_send_text("Servo moving for 5s", from, channel_index);
      return;
    } else if (strcmp(text, "ron") == 0) {
      Serial.println("Relay ON command recognized.");
      g_relay.on = false;
      // (Optional) mt_send_text("Relay turned ON", from, channel_index);
      return;
    }
    else if (strcmp(text, "roff") == 0) {
      Serial.println("Relay OFF command recognized.");
      g_relay.on = true;
      // (Optional) mt_send_text("Relay turned OFF", from, channel_index);
      return;
    }


  }
}


// This callback function will be called whenever the radio receives a text message
void text_message_callback(uint32_t from, uint32_t to,  uint8_t channel, const char* text) {
  const char* json = format_rx_json(from, to, channel, text);
  command_parser(from, to, channel, text);
  Serial.println(json);
  if (to == 0xFFFFFFFF){
    Serial.println("This is a BROADCAST message.");
  } else if (to == my_node_num){
    Serial.println("This is a DM to me!");
  } else {
    Serial.println("This is a DM to someone else.");
  }
}
//---------------=| End of Meshtastic Functions |=----------------------//

static void led_service() {
  if (led_light.active) {
    if (millis() < led_light.until_ms) {
      digitalWrite(LED_PIN, HIGH);
    } else {
      digitalWrite(LED_PIN, LOW);
      led_light.active = false;
    }
  } else {
    digitalWrite(LED_PIN, LOW);
  }
}

static void buzzer_service() {
  if (g_buzzer.active) {
    if (millis() < g_buzzer.until_ms) {
      digitalWrite(BUZZER_PIN, HIGH);
    } else {
      digitalWrite(BUZZER_PIN, LOW);
      g_buzzer.active = false;
    }
  } else {
    digitalWrite(BUZZER_PIN, LOW);
  }
}

static void oled_service() {
  if (!g_oled_msg.active) return;

  uint32_t now = millis();

  if (now < g_oled_msg.until_ms) {
    if (!g_oled_msg.drawn) {
      if (xSemaphoreTake(g_i2c_mutex, pdMS_TO_TICKS(50))) {
        display.clearDisplay();
        display.setTextSize(2);
        display.setTextColor(SSD1306_WHITE);
        display.setCursor(0, 8);
        display.println(F("Hello"));
        display.display();     // brief synchronous I2C flush
        xSemaphoreGive(g_i2c_mutex);
        g_oled_msg.drawn = true;
      }
    }
  } else {
    // Time’s up: clear display once, then deactivate
    if (xSemaphoreTake(g_i2c_mutex, pdMS_TO_TICKS(50))) {
      display.clearDisplay();
      display.display();
      xSemaphoreGive(g_i2c_mutex);
    }
    g_oled_msg.active = false;
    g_oled_msg.drawn  = false;
  }
}

static void co2_service() {
  // Take a consistent snapshot of the volatile values
  uint32_t hi = g_co2.high_us;
  uint32_t lo = g_co2.low_us;

  // Require both halves of the period
  if (hi == 0 || lo == 0) return;

  uint32_t T = hi + lo;
  if (T < 500 || T > 2000000) {
    // clearly bogus; ignore
    return;
  }

  // Spec formula (datasheet PWM: 2ms offset each edge, 0–5000ppm default range)
  // ppm = 5000 * (th - 2000) / (T - 4000)
  // Guard against tiny T
  if (T > 4000) {
    float ppm_f = 5000.0f * ( (float)hi - 2000.0f ) / ( (float)T - 4000.0f );
    if (ppm_f >= 0.0f && ppm_f <= 10000.0f) {
      co2_ppm = (uint32_t)(ppm_f + 0.5f);
      g_co2.valid = true;
      g_co2.last_period_ms = millis();
    }
  }

  // Staleness: if we don't see a period for a while, mark invalid (optional)
  if (g_co2.valid && (millis() - g_co2.last_period_ms) > CO2_TIMEOUT_MS) {
    g_co2.valid = false;
    co2_ppm = (uint32_t)-1;
  }
}


// Record edge timestamps and derive high/low durations.
// Keep it tiny; mark IRAM if preferred on ESP32.
void IRAM_ATTR co2_isr() {
  uint32_t now = micros();
  int level = digitalRead(g_co2.pin);

  if (level) {
    // Rising edge: LOW period just ended
    if (g_co2.last_fall_us != 0) {
      g_co2.low_us = now - g_co2.last_fall_us;
    }
    g_co2.last_rise_us = now;
  } else {
    // Falling edge: HIGH period just ended
    if (g_co2.last_rise_us != 0) {
      g_co2.high_us = now - g_co2.last_rise_us;
    }
    g_co2.last_fall_us = now;
  }
}

static void servo_service() {
  if (g_servo_state.active) {
    if (millis() < g_servo_state.until_ms) {
      g_servo.write(g_servo_state.target_deg);
    } else {
      // Time expired: return to default
      g_servo.write(g_servo_state.default_deg);
      g_servo_state.active = false;
    }
  }
}

static void relay_service() {
  if (g_relay.on) {
    digitalWrite(RELAY_PIN, RELAY_ACTIVE_LOW ? LOW : HIGH);
  } else {
    digitalWrite(RELAY_PIN, RELAY_ACTIVE_LOW ? HIGH : LOW);
  }
}

void setup() {
  // Try for up to five seconds to find a serial port; if not, the show must gox on
  Serial.begin(115200);

  g_servo.setPeriodHertz(50);  // SG90 expects 50 Hz
  g_servo.attach(SERVO_PIN, 500, 2400);  // min/max pulse widths in µs
  g_servo.write(g_servo_state.default_deg);

  pinMode(RELAY_PIN, OUTPUT);
  // Start OFF
  digitalWrite(RELAY_PIN, RELAY_ACTIVE_LOW ? HIGH : LOW);

  pinMode(CO2_PWM_PIN, INPUT);                 // MH-Z19B PWM output is push-pull
  attachInterrupt(digitalPinToInterrupt(CO2_PWM_PIN), co2_isr, CHANGE);


  // Preliminary LED light state
  pinMode(LED_PIN, OUTPUT);
  digitalWrite(LED_PIN, LOW);  // start off

  // Preliminary buzzer state
  pinMode(BUZZER_PIN, OUTPUT);
  digitalWrite(BUZZER_PIN, LOW);  // start off


  g_i2c_mutex = xSemaphoreCreateMutex();

  // Bring up I2C once here so OLED can init immediately
  Wire.begin(I2C_SDA_PIN, I2C_SCL_PIN);

  // Init OLED (non-fatal if missing)
  bool oled_ok = false;
  if (xSemaphoreTake(g_i2c_mutex, pdMS_TO_TICKS(250))) {
    oled_ok = display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
    if (oled_ok) {
      display.clearDisplay();
      display.display();
    }
    xSemaphoreGive(g_i2c_mutex);
  }
  if (!oled_ok) {
    Serial.println("OLED not found or init failed (continuing without it).");
  }

  startSensorTask();
  while(true) {
    if (Serial) break;
    if (millis() > 5000) {
      Serial.print("Couldn't find a serial port after 5 seconds, continuing anyway");
      break;
    }
  }

  Serial.print("Booted Meshtastic send/receive client in ");

// Change to 1 to use a WiFi connection
#if 0
  #include "arduino_secrets.h"
  Serial.print("wifi");
  mt_wifi_init(WIFI_CS_PIN, WIFI_IRQ_PIN, WIFI_RESET_PIN, WIFI_ENABLE_PIN, WIFI_SSID, WIFI_PASS);
#else
  Serial.print("serial");
  mt_serial_init(SERIAL_RX_PIN, SERIAL_TX_PIN, BAUD_RATE);
#endif
  Serial.println(" mode");

  randomSeed(micros());

  // Initial connection to the Meshtastic device
  mt_request_node_report(connected_callback);

  // Register a callback function to be called whenever a text message is received
  set_text_message_callback(text_message_callback);
  set_portnum_callback(portnum_callback);
  set_encrypted_callback(encrypted_callback);
}


void loop() {
  // Record the time that this loop began (in milliseconds since the device booted)
  uint32_t now = millis();

  // Run the Meshtastic loop, and see if it's able to send requests to the device yet
  bool can_send = mt_loop(now);

  // If we can send, and it's time to do so, send a text message and schedule the next one.
  if (can_send && now >= next_send_time) {
    mt_send_text("Hello, world!", dest, channel_index);
    next_send_time = now + SEND_PERIOD * 2000;
  }

    // Comms/UI only; sensors run in the task
  static uint32_t t0 = 0;
  if (millis() - t0 > 1000) {
    uint32_t a = plant_sensor_1, b = plant_sensor_2;
    Serial.printf("moisture A=%lu, B=%lu\n", (unsigned long)a, (unsigned long)b);
    t0 = millis();
  }
  oled_service();
  buzzer_service();
  led_service();
  co2_service();
  servo_service();
  relay_service();
}