Smart Classroom

/* Edge Impulse Arduino examples
 * Copyright (c) 2021 EdgeImpulse Inc.
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */

/* Includes ---------------------------------------------------------------- */
#include <test2_inferencing.h>
#include <Arduino_LSM9DS1.h>

#include <ArduinoBLE.h>
/* Constant defines -------------------------------------------------------- */
#define CONVERT_G_TO_MS2    9.80665f
#define MAX_ACCEPTED_RANGE  2.0f        // starting 03/2022, models are generated setting range to +-2, but this example use Arudino library which set range to +-4g. If you are using an older model, ignore this value and use 4.0f instead

#define NEURAL_IDLE 22
#define NEURAL_UNCERTAIN 33
#define NEURAL_UP 44
#define NEURAL_DOWN 55
#define NEURAL_SHIT 0


// BLE Battery Service
BLEService batteryService("9d1a777b-bdb9-4f72-bd86-dd6e3c991823");
// BLE Battery Level Characteristic
BLEUnsignedCharCharacteristic batteryLevelChar("930a735b-c678-4e08-a0af-948f38563503",  // standard 16-bit characteristic UUID
    BLERead | BLENotify); // remote clients will be able to get notifications if this characteristic changes


/*
 ** NOTE: If you run into TFLite arena allocation issue.
 **
 ** This may be due to may dynamic memory fragmentation.
 ** Try defining "-DEI_CLASSIFIER_ALLOCATION_STATIC" in boards.local.txt (create
 ** if it doesn't exist) and copy this file to
 ** `<ARDUINO_CORE_INSTALL_PATH>/arduino/hardware/<mbed_core>/<core_version>/`.
 **
 ** See
 ** (https://support.arduino.cc/hc/en-us/articles/360012076960-Where-are-the-installed-cores-located-)
 ** to find where Arduino installs cores on your machine.
 **
 ** If the problem persists then there's not enough memory for this model and application.
 */

/* Private variables ------------------------------------------------------- */
static bool debug_nn = false; // Set this to true to see e.g. features generated from the raw signal
static uint32_t run_inference_every_ms = 100;
static rtos::Thread inference_thread(osPriorityLow);
static float buffer[EI_CLASSIFIER_DSP_INPUT_FRAME_SIZE] = { 0 };
static float inference_buffer[EI_CLASSIFIER_DSP_INPUT_FRAME_SIZE];


//global variables for occupancy detection (0 prost, 1 zaseden, -1 napaka)
static int g_chairOccupancy = false;

//global raw neural output
static int g_rawNeural = 0; //see defines for values


/* Forward declaration */
void run_inference_background();

/**
* @brief      Arduino setup function
*/
void setup()
{
    // put your setup code here, to run once:
    Serial.begin(115200);
    delay(5000);
    Serial.println("Edge Impulse Inferencing Demo");

    pinMode(LED_BUILTIN, OUTPUT);

    if (!BLE.begin()) {
      Serial.println("starting BLE failed!");
      while (1);
    }

    BLE.setLocalName("pametna_ucilnica");
    BLE.setAdvertisedService(batteryService);
    batteryService.addCharacteristic(batteryLevelChar);
    BLE.addService(batteryService);

    BLE.advertise();



   
    Serial.println("starting AI...");

    if (!IMU.begin()) {
        ei_printf("Failed to initialize IMU!\r\n");
    }
    else {
        ei_printf("IMU initialized\r\n");
    }

    if (EI_CLASSIFIER_RAW_SAMPLES_PER_FRAME != 3) {
        ei_printf("ERR: EI_CLASSIFIER_RAW_SAMPLES_PER_FRAME should be equal to 3 (the 3 sensor axes)\n");
        return;
    }

    inference_thread.start(mbed::callback(&run_inference_background));

    
}


// to je funkcija ki vraa stanje zasedenosti stola (0 prost, 1 zaseden, -1 napaka)
//@ BLE programer: uporabli to funkcijo ko posiljas stanje na BLE
int aJeStolZaseden(void){
  return g_chairOccupancy;
}

// run this for every new neural output, calculates binary occupied/not occupied from neural output
//nastavlja g_chairOccupancy globalno spremenljivko
void occupancy_handler(int neuralOut){
  static int loopCtrl = 0;
  static uint32_t timing = millis();
  static uint32_t lastNotIdleTime = millis();
  //state machine
  switch(loopCtrl){

    //chair free state
    case 0:
      g_chairOccupancy = 0;
      if(neuralOut == NEURAL_DOWN){
        ei_printf("detected DN\n");
        timing = millis();
        loopCtrl = 1;
      }
    break;

    //waiting to settle after detected up
    case 1:
      if(neuralOut == NEURAL_IDLE && millis()-timing >= 3000){
        ei_printf("idle after DN ok\n");
        timing = millis();
        lastNotIdleTime = millis(); //reset lastNotIdleTime to current time before going to state occupied
        loopCtrl = 2;
      }
    break;

    //chair occupied case
    case 2:
      g_chairOccupancy = 1;
      if(neuralOut != NEURAL_IDLE){
        lastNotIdleTime = millis();
      }

      //go back to free if neural detected UP (or down, as two lables are often swapped)
      if(neuralOut == NEURAL_UP || neuralOut == NEURAL_DOWN){
        ei_printf("detected UP\n");
        timing = millis();
        loopCtrl = 3; //go back to wait for settle before going to free
      }

      //reset state to free if neural reported idle for 5 minutes
      else if (millis() - lastNotIdleTime >= 30000){
        ei_printf("no motion for 5min\n");
        timing = millis();
        loopCtrl = 0;
      }
    
    break;

    case 3:
      if(neuralOut == NEURAL_IDLE && millis()-timing >= 3000){
        ei_printf("idle after UP ok\n");
        timing = millis();
        loopCtrl = 0;
      }
    break;
  }
}

//converts first two letters of prediction lable (char array) to int
//hacky, but cont be bothered to find a better solution
int neuralTextToInt(char *firstLetterPointer){
    //uses these defines
//    #define NEURAL_IDLE 22
//    #define NEURAL_UNCERTAIN 33
//    #define NEURAL_UP 44
//    #define NEURAL_DOWN 55
//    #define NEURAL_SHIT 0
   int first = (int)*firstLetterPointer;
   int second = (int)*(firstLetterPointer+1);
   int lable = 0;
   if(first == 105 && second == 100) lable = NEURAL_IDLE;
   else if(first == 117 && second == 110) lable = NEURAL_UNCERTAIN;
   else if(first == 100 && second == 110) lable = NEURAL_DOWN;
   else if(first == 117 && second == 112) lable = NEURAL_UP;
   else lable = NEURAL_SHIT;
   return lable;
}

//use this to raw neural variable to serial in human readable format
void printLableToSerial (int lableInt){
//#define NEURAL_IDLE 22
//#define NEURAL_UNCERTAIN 33
//#define NEURAL_UP 44
//#define NEURAL_DOWN 55
//#define NEURAL_SHIT 

  if(lableInt == NEURAL_IDLE) ei_printf("neural output: IDLE\n");
  else if(lableInt == NEURAL_UNCERTAIN) ei_printf("neural output: UNCERTAIN\n");
  else if(lableInt == NEURAL_UP) ei_printf("neural output: UP\n");
  else if(lableInt == NEURAL_DOWN) ei_printf("neural output: DOWN\n");
  else if(lableInt == NEURAL_SHIT) ei_printf("neural output: ERROR\n");
  else ei_printf("neural output: EEERRORRR\n");
  
}


/**
* @brief      Printf function uses vsnprintf and output using Arduino Serial
*
* @param[in]  format     Variable argument list
*/
void ei_printf(const char *format, ...) {
   static char print_buf[1024] = { 0 };

   va_list args;
   va_start(args, format);
   int r = vsnprintf(print_buf, sizeof(print_buf), format, args);
   va_end(args);

   if (r > 0) {
       Serial.write(print_buf);
   }
}

/**
 * @brief Return the sign of the number
 * 
 * @param number 
 * @return int 1 if positive (or 0) -1 if negative
 */
float ei_get_sign(float number) {
    return (number >= 0.0) ? 1.0 : -1.0;
}

/**
 * @brief      Run inferencing in the background.
 */
void run_inference_background()
{
    // wait until we have a full buffer
    delay((EI_CLASSIFIER_INTERVAL_MS * EI_CLASSIFIER_RAW_SAMPLE_COUNT) + 100);

    // This is a structure that smoothens the output result
    // With the default settings 70% of readings should be the same before classifying.
    ei_classifier_smooth_t smooth;

    //todo: optimise smoothing params
    ei_classifier_smooth_init(&smooth, 10 /* no. of readings */, 7 /* min. readings the same */, 0.8 /* min. confidence */, 0.3 /* max anomaly */);

    while (1) {
        // copy the buffer
        memcpy(inference_buffer, buffer, EI_CLASSIFIER_DSP_INPUT_FRAME_SIZE * sizeof(float));

        // Turn the raw buffer in a signal which we can the classify
        signal_t signal;
        int err = numpy::signal_from_buffer(inference_buffer, EI_CLASSIFIER_DSP_INPUT_FRAME_SIZE, &signal);
        if (err != 0) {
            ei_printf("Failed to create signal from buffer (%d)\n", err);
            return;
        }

        // Run the classifier
        ei_impulse_result_t result = { 0 };

        err = run_classifier(&signal, &result, debug_nn);
        if (err != EI_IMPULSE_OK) {
            ei_printf("ERR: Failed to run classifier (%d)\n", err);
            return;
        }

//        // print the predictions
//        ei_printf("Predictions ");
//        ei_printf("(DSP: %d ms., Classification: %d ms., Anomaly: %d ms.)",
//            result.timing.dsp, result.timing.classification, result.timing.anomaly);
//        ei_printf(": ");

        // ei_classifier_smooth_update yields the predicted label
        const char *prediction = ei_classifier_smooth_update(&smooth, &result);

        //set global raw neural output variable
        g_rawNeural = neuralTextToInt((char*)prediction);
        //printLableToSerial(g_rawNeural);
        occupancy_handler(neuralTextToInt((char*)prediction)); //run occupancy handler

        
//        ei_printf("%s ", prediction);
//        // print the cumulative results
//        ei_printf(" [ ");
//        for (size_t ix = 0; ix < smooth.count_size; ix++) {
//            ei_printf("%u", smooth.count[ix]);
//            if (ix != smooth.count_size + 1) {
//                ei_printf(", ");
//            }
//            else {
//              ei_printf(" ");
//            }
//        }
//        ei_printf("]\n");

        delay(run_inference_every_ms);
    }

    ei_classifier_smooth_free(&smooth);
}

/**
* @brief      Get data and run inferencing
*
* @param[in]  debug  Get debug info if true
*/
void loop()
{
    BLEDevice central = BLE.central();
    if(central.connected()) Serial.println("BLE connected");
    while (central.connected()) {
      
        // Determine the next tick (and then sleep later)
        uint64_t next_tick = micros() + (EI_CLASSIFIER_INTERVAL_MS * 1000);

        // roll the buffer -3 points so we can overwrite the last one
        numpy::roll(buffer, EI_CLASSIFIER_DSP_INPUT_FRAME_SIZE, -3);

        // read to the end of the buffer
        IMU.readAcceleration(
            buffer[EI_CLASSIFIER_DSP_INPUT_FRAME_SIZE - 3],
            buffer[EI_CLASSIFIER_DSP_INPUT_FRAME_SIZE - 2],
            buffer[EI_CLASSIFIER_DSP_INPUT_FRAME_SIZE - 1]
        );

        for (int i = 0; i < 3; i++) {
            if (fabs(buffer[EI_CLASSIFIER_DSP_INPUT_FRAME_SIZE - 3 + i]) > MAX_ACCEPTED_RANGE) {
                buffer[EI_CLASSIFIER_DSP_INPUT_FRAME_SIZE - 3 + i] = ei_get_sign(buffer[EI_CLASSIFIER_DSP_INPUT_FRAME_SIZE - 3 + i]) * MAX_ACCEPTED_RANGE;
            }
        }

        buffer[EI_CLASSIFIER_DSP_INPUT_FRAME_SIZE - 3] *= CONVERT_G_TO_MS2;
        buffer[EI_CLASSIFIER_DSP_INPUT_FRAME_SIZE - 2] *= CONVERT_G_TO_MS2;
        buffer[EI_CLASSIFIER_DSP_INPUT_FRAME_SIZE - 1] *= CONVERT_G_TO_MS2;

        // and wait for next tick
        uint64_t time_to_wait = next_tick - micros();
        delay((int)floor((float)time_to_wait / 1000.0f));
        delayMicroseconds(time_to_wait % 1000);


        //print chair state every second
        static uint32_t lastprint = millis();
        if(millis() - lastprint > 1000){
          if(aJeStolZaseden()==1){
            ei_printf("ZASEDENO\n");
          }
          else{
            ei_printf("PROSTO\n");
          }
          lastprint = millis();
        }
        
    }
}

#if !defined(EI_CLASSIFIER_SENSOR) || EI_CLASSIFIER_SENSOR != EI_CLASSIFIER_SENSOR_ACCELEROMETER
#error "Invalid model for current sensor"
#endif