LaundReminder

#include <mutex>

/**
 * LaundReminder
 * 
 * Author: Manuel Stein (manuel.stein@web.de)
 * Copyright: Apache License 2.0
 * 
 * Library dependencies:
 * - SPARKFUNLSM9DS (v1.1.3)
 * - MQTT (v0.4.8)
 * 
 * LaundReminder is an embedded device that can be put on any washing machine to monitor its operation (and remind the user to remove the laundry).
 * The LaundReminder application can be switched on and off with a button that connects it to Losant.
 * While on, it uses a hardware timer to accurately read the 3-dim acceleration vector from the LSM9DS1 and stores it in a buffer
 * Every time the buffer has reached one block for processing, it sets an index to the position in the buffer for the main loop to process,
 * When the application is on, the main loop processes a new buffer block if available and uses a Goertzel filter to extract the power of a frequency signal.
 * The first 60 target frequency power samples (i.e. 1 minute) are averaged to calibrate the device.
 * After calibration, it tracks two values:
 * - the 1-minute target frequency power average is compared to the calibration average to detect whether the machine is washing
 * - the acceleration norm maximum of every second is monitored to detect impacts
 * After a washing, i.e. when the machine has returned from washing to not washing, a reminder sends a status update with the time the machine has stopped to Losant
 * Any impact clears the timer and puts the application back to idling where it awats another washing
 * 
 * The application has been developed as part of the Embedded Systems Engineering Workshop of the Master of Distributed Computing Systems Engineering course at Brunel University London.
 **/
//------------------------------------------------------------------------------
// COMPONENTS
//------------------------------------------------------------------------------
#define LED_PIN D7
#define LED_R D6
#define LED_Y D5
#define LED_G D4
#define BUTTON_PIN D3

//------------------------------------------------------------------------------
// LOSANT
//------------------------------------------------------------------------------
#include <MQTT.h>

// Credentials for losant - see access keys
#define LOSANT_BROKER "broker.losant.com"
#define LOSANT_DEVICE_ID "<<LOSANT_DEVICE_ID>>"
#define LOSANT_ACCESS_KEY "<<LOSANT_ACCESS_KEY>>"
#define LOSANT_ACCESS_SECRET "<<LOSANT_ACCESS_SECRET>>"

char msg[50];

// Topic used to subscribe to Losant commands.
String MQTT_TOPIC_COMMAND =
    String::format("losant/%s/command", LOSANT_DEVICE_ID);

// Topic used to publish state to Losant.
String MQTT_TOPIC_STATE =
    String::format("losant/%s/state", LOSANT_DEVICE_ID);
    
// MQTT client.
MQTT client("broker.losant.com", 1883, callback);

void callback(char* topic, byte* payload, unsigned int length) {
    
    Serial.print("Received command");
}

int LAUNDRYTIME = 1; // remind every [x] minutes
int laundryCount=0; // minutes the laundry has been in the machine after it stopped

void laundremind() {
    laundryCount += LAUNDRYTIME;
    sprintf(msg,"{\"data\":{\"ended\":%d}}", laundryCount);
    client.publish(MQTT_TOPIC_STATE, msg);
}
Timer laundrytimer(1000*60*LAUNDRYTIME, laundremind, false);

int set_laundrytime(String minutes) {
    LAUNDRYTIME = minutes.toInt();
    Serial.printlnf("[INFO] set new reminder interval of %d minutes", LAUNDRYTIME);
}
//------------------------------------------------------------------------------
// LSM9DS1 settings
//------------------------------------------------------------------------------
#include <SparkFunLSM9DS1.h>

LSM9DS1 imu;

#define LSM9DS1_M	0x1E // Would be 0x1C if SDO_M is LOW
#define LSM9DS1_AG	0x6B // Would be 0x6A if SDO_AG is LOW

#define PRINT_CALCULATED
#define PRINT_SPEED 250 // 250 ms between prints
#define DECLINATION -8.58 // Declination (degrees) in Boulder, CO.

/*enum Ascale {  // set of allowable accel full scale settings
  AFS_2G = 0,
  AFS_16G,
  AFS_4G,
  AFS_8G
};

enum Aodr {  // set of allowable gyro sample rates
  AODR_PowerDown = 0,
  AODR_10Hz,
  AODR_50Hz,
  AODR_119Hz,
  AODR_238Hz,
  AODR_476Hz,
  AODR_952Hz
};

enum Abw {  // set of allowable accewl bandwidths
   ABW_408Hz = 0,
   ABW_211Hz,
   ABW_105Hz,
   ABW_50Hz
};

uint8_t Ascale = AFS_2G;     // accel full scale
uint8_t Aodr = AODR_238Hz;   // accel data sample rate
uint8_t Abw = ABW_50Hz;      // accel data bandwidth
*/

//------------------------------------------------------------------------------
// ACCELERATION
//------------------------------------------------------------------------------

// The analysis always uses half the buffer while recording happens in the other half
// A buffer rate of 2 equals 2ms, i.e. a sample rate of 500Hz, which means the analysis can detect frequencies up to 250Hz
// Increasing the buffer length (BUFLEN) will increase the resolution of the analyzed spectrum and slow the analysis rate, e.g. BUFLEN = 4000 will use
// 1000 samples per analysis, which takes 2 seconds to record and provide a resolution of 500/1000 ~.5Hz
#define BUFLEN 2000
#define BUFRATE 2 // sample write rate ms
float buffer[BUFLEN];
int rpos=-1; // reading position [idx]
int wpos=0; // writing position [idx]
int section=0;
unsigned long last=0;

/**
 * acceleration() - invoked by the hardware timer
 */
void acceleration() {
    // read sensor and write normed acceleration to buffer
    imu.readAccel();
    // range conversion [0,65535] -> [-2G,2G]
    float x = ((int)imu.ax) / 16384.0;
    float y = ((int)imu.ay) / 16384.0;
    float z = ((int)imu.az) / 16384.0;
    // Euclidean 3-norm
    buffer[wpos] = sqrt( x*x + y*y + z*z );
    // loop counter
    wpos++;
    if(wpos >= BUFLEN)
        wpos = 0;
    // if we've finished with one quarter, inform main thread
    if(section==0 && wpos >= BUFLEN/4) {
        // tell main thread to read first quarter
        rpos=0;
        section=1;
    } else if(section==1 && wpos >= BUFLEN/2) {
        // tell main thread to read second quarter
        rpos=BUFLEN/4;
        section=2;
    } else if(section==2 && wpos >= BUFLEN*3/4) {
        // tell main thread to read third quarter
        rpos=BUFLEN/2;
        section=3;
    } else if(section==3 && wpos < BUFLEN/4) {
        // tell main thread to read last quarter
        rpos=BUFLEN*3/4;
        section=0;
    }
}

TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; 
NVIC_InitTypeDef        NVIC_InitStructure; 

/**
 * overwrite hardware timer function to use it
 */
void Wiring_TIM3_Interrupt_Handler_override()
{
    if (TIM_GetITStatus(TIM3,TIM_IT_Update) != RESET)
    {
        TIM_ClearITPendingBit(TIM3, TIM_IT_Update);
        //Interrupt code here
        acceleration();
    }
}

/**
 * setup_acceleration()
 * 
 * configures the hardware timer; there's still a problem that the timer does not work after the device has been powered up.
 * Maybe it is some registers not being set to zero. Pushing the Particle's reset button does the trick
 */
void setup_acceleration() {
    // Steps from http://www.disca.upv.es/aperles/arm_cortex_m3/curset/STM32F4xx_DSP_StdPeriph_Lib_V1.0.1/html/group___t_i_m___group1.html
    
    /* TIM3 clock enable */
    RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);
    
    /* Timer Base configuration */
    TIM_TimeBaseStructure.TIM_Prescaler = (uint16_t)(SystemCoreClock / 1000000) - 1; // 1MHz rate
    TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
    TIM_TimeBaseStructure.TIM_Period = BUFRATE * 500 - 1; // ticktock, goes every tick (e.g. 1000 -> every 2ms)
    TIM_TimeBaseStructure.TIM_ClockDivision = 0;
    TIM_TimeBaseInit(TIM3, &TIM_TimeBaseStructure);

    /* ISR configuration */
    NVIC_InitStructure.NVIC_IRQChannel = TIM3_IRQn;
    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=0;
    NVIC_InitStructure.NVIC_IRQChannelSubPriority=1;
    NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
    NVIC_Init(&NVIC_InitStructure);
    
    TIM_ITConfig(TIM3, TIM_IT_Update, ENABLE);

    /* TIM3 Counter Enable */
    attachSystemInterrupt(SysInterrupt_TIM3_Update, Wiring_TIM3_Interrupt_Handler_override);
}

#define samplingRate 500.0
double targetFrequency=100.0;
float coeff;
float s_prev;
float s_prev2;
float s;
float smax;
unsigned long lastacc=0;

/**
 * acceleration_processing()
 * the main loop must call this function repeatedly. If there is new data in the buffer to process, it will update the magnitude of the target frequency signal power and the maximum acceleration vector norm
 * The acceleration vector norms are stored in the buffer of samples
 * 
 * float &mag - target signal power in the processed buffer of samples
 * float &maxa - maximum acceleration withing the bufer of samples
 */
int acceleration_processing(float &mag, float &maxa) {
    if(rpos < 0)
      return FALSE;

    int r = rpos;
    int n = BUFLEN/4;
    float* samples = (float*)(buffer+r);

    //### SAMPLE STATISTICS
    coeff = 2.0 * cos(2 * PI * targetFrequency / samplingRate);
    s_prev = 0.0;
    s_prev2 = 0.0;
    s;
    smax=0.0;
    for(int i=0;i<BUFLEN/4;i++) {
        // goertzel
        s = samples[i] + coeff * s_prev - s_prev2;
        s_prev2 = s_prev;
        s_prev = s;
        // maximum
        if(samples[i] > smax)
            smax=samples[i];
    }
    maxa=smax;
    mag = sqrt( s_prev2*s_prev2 + s_prev*s_prev - coeff*s_prev*s_prev2 );
    unsigned long m = millis();
    Serial.printlnf("%15lu,maxa:%10.5f,mag:%10.5f",(m-lastacc),maxa,mag);
    lastacc=m;

    // done reading
    rpos=-1;

    return TRUE;
}


//------------------------------------------------------------------------------
// STATE MACHINES
//------------------------------------------------------------------------------
/*
The LaundReminder has a hierarchical state machine
Its application state machine manages the connectivity to Losant and the app being switched on or off
When on, the monitoring state machine manages the state that the app thinks the washin machine is in
*/
// Application States
enum AppState { STATE_OFF, STATE_CONNECTING, STATE_ON, STATE_DISCONNECTING };
AppState astate = STATE_OFF;

// Monitor States
enum MonState { STATE_CALIBRATING, STATE_IDLE, STATE_WASHING, STATE_ENDED };
MonState mstate = STATE_CALIBRATING;

std::mutex sm;

#define BLINK 300
// Output associated with all state combinations
void output() {
    switch(astate) {
        case STATE_OFF: {
    	    digitalWrite(LED_R,LOW);
    	    digitalWrite(LED_Y,LOW);
    	    digitalWrite(LED_G,LOW);
            break;
        }
        case STATE_CONNECTING: {
    	    digitalWrite(LED_R,LOW);
    	    digitalWrite(LED_Y,HIGH);//blink
    	    digitalWrite(LED_G,HIGH);
    	    delay(BLINK);
    	    digitalWrite(LED_Y,LOW);
    	    delay(BLINK);
    	    digitalWrite(LED_Y,HIGH);
    	    delay(BLINK);
    	    digitalWrite(LED_Y,LOW);
            break;
        }
        case STATE_ON: {
            switch(mstate) {
                case STATE_CALIBRATING: {
            	    digitalWrite(LED_R,HIGH);//blink
            	    digitalWrite(LED_Y,LOW);
    	            digitalWrite(LED_G,HIGH);
                    delay(BLINK);
                    digitalWrite(LED_R,LOW);
                    delay(BLINK);
                    digitalWrite(LED_R,HIGH);
                    delay(BLINK);
                    digitalWrite(LED_R,LOW);
                    break;
                }
                case STATE_IDLE: {
            	    digitalWrite(LED_R,LOW);
            	    digitalWrite(LED_Y,LOW);
    	            digitalWrite(LED_G,HIGH);
                    break;
                }
                case STATE_WASHING: {
            	    digitalWrite(LED_R,LOW);
            	    digitalWrite(LED_Y,HIGH);
    	            digitalWrite(LED_G,LOW);
                    break;
                }
                case STATE_ENDED: {
            	    digitalWrite(LED_R,HIGH);
            	    digitalWrite(LED_Y,LOW);
    	            digitalWrite(LED_G,LOW);
                    break;
                }
                default: {
            	    digitalWrite(LED_R,HIGH);
            	    digitalWrite(LED_Y,HIGH);
    	            digitalWrite(LED_G,HIGH);
                    delay(BLINK);
            	    digitalWrite(LED_R,LOW);
            	    digitalWrite(LED_Y,LOW);
    	            digitalWrite(LED_G,LOW);
                    delay(BLINK);
            	    digitalWrite(LED_R,HIGH);
            	    digitalWrite(LED_Y,HIGH);
    	            digitalWrite(LED_G,HIGH);
                    delay(BLINK);
            	    digitalWrite(LED_R,LOW);
            	    digitalWrite(LED_Y,LOW);
    	            digitalWrite(LED_G,LOW);
                    break;
                }
            }
            break;
        }
        case STATE_DISCONNECTING: {
    	    digitalWrite(LED_R,LOW);
    	    digitalWrite(LED_Y,HIGH);//blink
    	    digitalWrite(LED_G,LOW);
    	    delay(BLINK);
    	    digitalWrite(LED_Y,LOW);
    	    delay(BLINK);
    	    digitalWrite(LED_Y,HIGH);
    	    delay(BLINK);
    	    digitalWrite(LED_Y,LOW);
            break;
        }
        default: {
    	    digitalWrite(LED_R,HIGH);
    	    digitalWrite(LED_Y,HIGH);
            digitalWrite(LED_G,HIGH);
            delay(BLINK);
    	    digitalWrite(LED_R,LOW);
    	    digitalWrite(LED_Y,LOW);
            digitalWrite(LED_G,LOW);
            delay(BLINK);
    	    digitalWrite(LED_R,HIGH);
    	    digitalWrite(LED_Y,HIGH);
            digitalWrite(LED_G,HIGH);
            delay(BLINK);
    	    digitalWrite(LED_R,LOW);
    	    digitalWrite(LED_Y,LOW);
            digitalWrite(LED_G,LOW);
            break;
        }
    }
}

double IMPACTTHRESHOLD = 1.05;
double RATIOTHRESHOLD = 3.0;
bool trained = false;
bool washing = false;
bool impact = false;

int set_impactthreshold(String threshold) {
    IMPACTTHRESHOLD = threshold.toFloat();
    Serial.printlnf("[INFO] set new impact threshold of %f", IMPACTTHRESHOLD);
}
int set_washingthreshold(String threshold) {
    RATIOTHRESHOLD = threshold.toFloat();
    Serial.printlnf("[INFO] set new signal power ratio threshold of %f", RATIOTHRESHOLD);
}

// Application Transition actions
void atrans(AppState anew) {
    static float mtrace=0.0;
    static float mrest=-1.0;
    static float amax=0.0;
    static int mcount=0;
    AppState aold = astate;
    switch(anew) {
        case STATE_OFF: {
            if(aold != STATE_OFF) {
                Serial.println("[INFO] Entering STATE_OFF");
            } else {
                Serial.println("[INFO] STATE_OFF - waiting for button press");
                delay(1000);
            }
            break;
        }
        case STATE_CONNECTING: {
            if(aold != STATE_CONNECTING) {
                Serial.println("[INFO] Entering STATE_CONNECTING");
            }
            // Connect to Losant
            client.connect(
                LOSANT_DEVICE_ID,
                LOSANT_ACCESS_KEY,
                LOSANT_ACCESS_SECRET);
            break;
        }
        case STATE_ON: {
            if(aold != STATE_ON) {
                Serial.println("[INFO] Entering STATE_ON");
                // enable acceleration recording
                TIM_Cmd(TIM3, ENABLE);

                // Subscribe to Losant commands
                client.subscribe(MQTT_TOPIC_COMMAND);
            } else {
                /**
                 * Acceleration processing provides the 100Hz signal power and the acceleration amplitude (max movement) if data are available
                 * If it has new values (there should be new data every second, depending on sampling parameters), the maximum acceleration
                 * and the average signal power for one minute is taken.
                 * If untrained, the first average of signal power serves as the "resting" signal power, the remainder is used for monitoring.
                 * 
                 * both resting and monitoring values are reported to Losant
                 * 
                 * Every monitored minute power is compared to the resting power (RATIOTHRESHOLD) to detect washing 
                 * The maximum acceleration is compared to IMPACTTHRESHOLD to detect impacts
                 */
                float X,amp;
                if(acceleration_processing(X,amp)) {
                    if(X < 50.0) {
                        mcount++;
                        mtrace += X / 60.0;

                        if(amp > amax)
                            amax = amp;

                        if(amp > IMPACTTHRESHOLD)
                            impact = true;
                        else
                            impact = false;

                        if(mcount >= 60) { // every minute
                            trained = true;
                            if(mrest < 0.0) {
                                mrest = mtrace;
                                // Log to losant
                                char msg[50];
                                sprintf(msg,"{\"data\":{\"resting\":%.5f}}", mrest);
                                client.publish(MQTT_TOPIC_STATE, msg);
                            } else {
                                // Log to losant
                                char msg[50];
                                sprintf(msg,"{\"data\":{\"signal\":%.5f,\"amax\":%.5f,\"ratio\":%.5f}}", mtrace, amax, (mtrace / mrest));
                                client.publish(MQTT_TOPIC_STATE, msg);

                                if(mtrace / mrest > RATIOTHRESHOLD)
                                    washing = true;
                                else
                                    washing = false;
                            }
                            mtrace = 0.0;
                            mcount = 0;
                            amax = 0.0;
                        }
                        mnext(trained,washing,impact);
                    } // else ignore sample of this second
                }
                //Serial.println("[INFO] STATE_ON - processing");
                delay(100);
            }
            break;
        }
        case STATE_DISCONNECTING: {
            if(aold != STATE_DISCONNECTING) {
                Serial.println("[INFO] Entering STATE_DISCONNECTING");
                // Disable acceleration recording
                TIM_Cmd(TIM3, DISABLE);
            }
            // Disconnect from Losant
            client.disconnect();
            break;
        }
        default: {
            Serial.println("[ERROR] Unknown Application State to transition to");
            break;
        }
    }
    astate = anew;
    output();
}

// Application Next State Function S x I -> S
void anext(bool button, bool connected) {
    sm.lock();
    switch(astate) {
        case STATE_OFF: {
            if(button)
                atrans(STATE_CONNECTING);
            else// !button - remain OFF
                atrans(STATE_OFF);
            break;
        }
        case STATE_CONNECTING: {
            if(button)
                atrans(STATE_DISCONNECTING);
            else // !button
                if(connected)
                    atrans(STATE_ON);
                else//!connected - remain CONNECTING
                    atrans(STATE_CONNECTING);
            break;
        }
        case STATE_ON: {
            if(button)
                atrans(STATE_DISCONNECTING);
            else // !button
                if(!connected)
                    atrans(STATE_CONNECTING);
                else//connected - remain ON
                    atrans(STATE_ON);
            break;
        }
        case STATE_DISCONNECTING: {
            if(!connected)
                atrans(STATE_OFF);
            else//connected - remain DISCONNECTING
                atrans(STATE_DISCONNECTING);
            break;
        }
        default: {
            Serial.println("[ERROR] Unknown Application State to act upon");
            break;
        }
    }
    sm.unlock();
}

// Monitor Transition actions
void mtrans(MonState mnew) {
    mstate = mnew;
    output();
    switch(mnew) {
        case STATE_CALIBRATING: {
            Serial.println("[INFO] Entering STATE_CALIBRATING");
            client.publish(MQTT_TOPIC_STATE, "{\"data\":{\"state\":0,\"name\":\"CALIBRATING\"}}");
            break;
        }
        case STATE_IDLE: {
            Serial.println("[INFO] Entering STATE_IDLE");
            client.publish(MQTT_TOPIC_STATE, "{\"data\":{\"state\":1,\"name\":\"IDLE\"}}");
            if(laundrytimer.isActive())
                laundrytimer.stop();
                laundryCount=0;
            break;
        }
        case STATE_WASHING: {
            Serial.println("[INFO] Entering STATE_WASHING");
            client.publish(MQTT_TOPIC_STATE, "{\"data\":{\"state\":2,\"name\":\"WASHING\"}}");
            if(laundrytimer.isActive())
                laundrytimer.stop();
                laundryCount=0;
            break;
        }
        case STATE_ENDED: {
            Serial.println("[INFO] Entering STATE_ENDED");
            client.publish(MQTT_TOPIC_STATE, "{\"data\":{\"state\":3,\"name\":\"ENDED\"}}");
            if(!laundrytimer.isActive())
                laundrytimer.start();
            break;
        }
        default: {
            Serial.println("[ERROR] Unknown Monitor State");
            break;
        }
    }
}

// Monitor Next State Function S x I -> S
void mnext(bool trained, bool washing, bool impact) {
    switch(mstate) {
        case STATE_CALIBRATING: {
            if(trained)
                mtrans(STATE_IDLE);
            // else !trained - remain CALIBRATING
            break;
        }
        case STATE_IDLE: {
            if(!trained)
                mtrans(STATE_CALIBRATING);
            else // trained
                if(washing)
                    mtrans(STATE_WASHING);
                //else !washing - remain IDLE
            break;
        }
        case STATE_WASHING: {
            if(!trained)
                mtrans(STATE_CALIBRATING);
            else // trained
                if(!washing)
                    mtrans(STATE_ENDED);
                //else washing - remain WASHING
            break;
        }
        case STATE_ENDED: {
            if(!trained)
                mtrans(STATE_CALIBRATING);
            else // trained
                if(washing)
                    mtrans(STATE_WASHING);
                else
                    if(impact)
                        mtrans(STATE_IDLE);
                    //else !impact - remain ENDED
            break;
        }
        default: {
            Serial.println("[ERROR] Unknown Monitor State to act upon");
            break;
        }
    }
}

/**
 * BUTTON interrupt
 * 
 * If the button is pressed, the eponymous interrupt handler function is called
 * It keeps the last time it has been triggered to avoid one slow pushing of a button to cause multiple state changes,
 * so the resulting state machine input of a "button pressed" is recognized correctly
 */
bool bPressed=false;

void buttonPressed(void) {
    static uint16_t lastPressed=0;
    uint16_t ms = millis();
    if(ms-lastPressed < 1000) {
        Serial.println("[DEBUG] Duplicate button press signal");
        lastPressed = ms;
        return;
    } else {
        bPressed = true;
        Serial.println("[INFO] Button pressed!");
    }
}

void loop() {
    // Loop MQTT client to retrieve commands
    client.loop();

    uint16_t ms = millis();
    bool bp=bPressed; // get value of bPressed first or it will be false before we can get it (out of order incredible!)
    bPressed = false;
    anext(bp,client.isConnected());
    Particle.process();
}

STARTUP(Serial.begin(115200));
void setup() {
    Serial.println("Setup start");

    Particle.variable("reminder", LAUNDRYTIME);
    Particle.function("setReminder", set_laundrytime);
    Particle.function("setImpact", set_impactthreshold);
    Particle.function("setRatio", set_washingthreshold);
    Particle.variable("signalFreq", targetFrequency);
    Particle.variable("impactThresh", IMPACTTHRESHOLD);
    Particle.variable("ratioThresh", RATIOTHRESHOLD);
    // LED SETUP
    LOG(INFO,"Setup LEDs");
    pinMode(LED_PIN, OUTPUT);
    pinMode(LED_R, OUTPUT);
    pinMode(LED_Y, OUTPUT);
    pinMode(LED_G, OUTPUT);
    // LED test
    Serial.println("[INFO] Testing LEDs");
    digitalWrite(LED_R, HIGH);
    delay(BLINK);
    digitalWrite(LED_R, LOW);
    digitalWrite(LED_Y, HIGH);
    delay(BLINK);
    digitalWrite(LED_Y, LOW);
    digitalWrite(LED_G, HIGH);
    delay(BLINK);
    digitalWrite(LED_G, LOW);
    delay(BLINK);
    digitalWrite(LED_R, HIGH);
    digitalWrite(LED_Y, HIGH);
    digitalWrite(LED_G, HIGH);
    delay(BLINK);
    digitalWrite(LED_R, LOW);
    digitalWrite(LED_Y, LOW);
    digitalWrite(LED_G, LOW);

    // BUTTON SETUP
    Serial.println("[INFO] Setup Button");
    pinMode(BUTTON_PIN, INPUT_PULLUP);
    attachInterrupt(BUTTON_PIN, buttonPressed, FALLING);

    // LSM9DS1 SETUP
    imu.settings.device.commInterface = IMU_MODE_I2C;
    imu.settings.device.mAddress = LSM9DS1_M;
    imu.settings.device.agAddress = LSM9DS1_AG;
    if (!imu.begin()) {
        Serial.println("[ERROR] Unable to initialize the LSM9DS0. Check wiring!");
        //atrans(STATE_OFF);
        return;
    }
    Serial.println("[INFO] LSM9DS1 ready");        

    setup_acceleration();
    Serial.println("[INFO] Acceleration timers ready to go");        

    //transition(STATE_INIT);
    Serial.println("[INFO] Setup done");

    uint32_t freemem = System.freeMemory();
    Serial.print("free memory: ");
    Serial.println(freemem);
}