compartment-monitor.ino

#include <WioLTEforArduino.h>
#include <GroveDriverPack.h>
#include <math.h>

const unsigned long POWER_SYPPLY_WAITTIME = 500;    // ms
const unsigned long MEASUREMENT_INTERVAL = 400;     // ms
const unsigned long ENTER_DECISION_TIME = 2000;     // ms
const unsigned long LEAVE_DECISION_TIME = 20000;    // ms
const unsigned long MOVEMENT_DECISION_TIME = 40000; // ms
const float STATE_CHANGE_DISTANCE = 800.0;          // mm
const float MOVEMENT_RANGE = 3.0;                   // mm
const long RECEIVE_TIMEOUT = 10000;                 // ms

static WioLTE Wio;
static GroveBoard Board;
static GroveUltrasonicRanger Ranger(&Board.D38);

bool send_access_status(const char *status)
{
  int connect_id = -1;
  int length = -1;
  bool ret = true;
  char data[100] = {0};

  SerialUSB.println("### Open.");
  connect_id = Wio.SocketOpen("funnel.soracom.io", 23080, WIOLTE_UDP);
  if (connect_id < 0)
  {
    SerialUSB.println("### ERROR! ###");
    return false;
  }

  sprintf(data, "{\"status\":\"%s\"}", status);
  SerialUSB.println("### Send.");
  SerialUSB.print("Send: ");
  SerialUSB.println(data);

  if (!Wio.SocketSend(connect_id, data))
  {
    SerialUSB.println("### ERROR! ###");
    ret = false;
    goto close;
  }

  SerialUSB.println("### Receive.");
  length = Wio.SocketReceive(connect_id, data, sizeof(data), RECEIVE_TIMEOUT);
  if (length < 0)
  {
    SerialUSB.println("### ERROR! ###");
    ret = false;
    goto close;
  }
  if (length == 0)
  {
    SerialUSB.println("### RECEIVE TIMEOUT! ###");
    ret = false;
    goto close;
  }

  SerialUSB.print("Receive: ");
  SerialUSB.println(data);

close:
  SerialUSB.println("### Close.");
  if (!Wio.SocketClose(connect_id))
  {
    SerialUSB.println("### ERROR! ###");
    return false;
  }

  return ret;
}

void setup()
{
  Wio.Init();
  SerialUSB.println("--- START");
  Board.D38.Enable();
  Ranger.Init();

  SerialUSB.println("### Power supply ON.");
  Wio.PowerSupplyLTE(true);
  delay(POWER_SYPPLY_WAITTIME);

  SerialUSB.println("### Turn on or reset.");
  if (!Wio.TurnOnOrReset())
  {
    SerialUSB.println("### ERROR! ###");
    return;
  }

  SerialUSB.println("### Connecting to \"soracom.io\".");
  if (!Wio.Activate("soracom.io", "sora", "sora"))
  {
    SerialUSB.println("### ERROR! ###");
    return;
  }

  Wio.LedSetRGB(0, 16, 0);
  SerialUSB.println("### Setup completed.");
}

void loop()
{
  static boolean movement = true;
  static boolean in_room = false;
  static boolean within_distance = false;
  static boolean prev_within_distance = false;
  static boolean same_distance = false;
  static boolean prev_same_distance = false;
  static unsigned long same_at = 0;
  static unsigned long inside_at = 0;
  static unsigned long outside_at = 0;
  static float prev_distance = 0;
  float distance = 0;

  Ranger.Read();
  distance = Ranger.Distance;
  SerialUSB.print("Distance: ");
  SerialUSB.println(distance);

  prev_same_distance = same_distance;
  same_distance = (fabs(distance - prev_distance) < MOVEMENT_RANGE);
  prev_distance = distance;

  if (same_distance)
  {
    if (!prev_same_distance)
    {
      same_at = millis();
    }

    if (movement && (millis() - same_at >= MOVEMENT_DECISION_TIME))
    {
      SerialUSB.println("--- NO MOVEMENT");
      Wio.LedSetRGB(0, 0, 16);
      movement = false;

      if (in_room)
      {
        SerialUSB.println("--- LEAVE");
        send_access_status("leave");
        within_distance = false;
        in_room = false;
      }
    }
  }
  else
  {
    if (!movement)
    {
      SerialUSB.println("--- MOVING");
      Wio.LedSetRGB(0, 16, 0);
      movement = true;
    }
  }

  if (!movement)
  {
    goto loop_end;
  }

  prev_within_distance = within_distance;
  within_distance = (distance <= STATE_CHANGE_DISTANCE);

  if (within_distance)
  {
    if (!prev_within_distance)
    {
      inside_at = millis();
    }

    if (!in_room && (millis() - inside_at >= ENTER_DECISION_TIME))
    {
      SerialUSB.println("--- ENTER");
      send_access_status("enter");
      Wio.LedSetRGB(16, 0, 0);
      in_room = true;
    }
  }
  else
  {
    if (prev_within_distance)
    {
      outside_at = millis();
    }

    if (in_room && (millis() - outside_at >= LEAVE_DECISION_TIME))
    {
      SerialUSB.println("--- LEAVE");
      send_access_status("leave");
      Wio.LedSetRGB(0, 16, 0);
      in_room = false;
    }
  }

loop_end:
  delay(MEASUREMENT_INTERVAL);
}