I.S.I - Intelligente Schwerkraft Infusion

/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Neo-Circle stuff (copy from: https://github.com/MrDio/IoT-Prototyping-Framework/tree/master/IoT-Eco-NeoPIXEL_Ring_Sample)
//

/**
 * 
 * Wiring for neo-pixel:
 * 
 * PHO (D2) - NEO (Data Input)
 * PHO (GND) - NEO (GND)
 * PHO (3V3) - NEO (5V)
 * 
 */

// This #include statement was automatically added by the Particle IDE.
#include <neopixel.h>

#include "HX711ADC.h"
#include "application.h"
#include "math.h"

class IoTEcoSys_NeoPIXEL_Ring
{
private:

public:
    IoTEcoSys_NeoPIXEL_Ring(int pixel);
    IoTEcoSys_NeoPIXEL_Ring();
    bool init();
    int setGlRGB( String color);
    void neoRingClockWise(int circles, int del, String color);
    void neoRingFillClockWise(int showuptime, int del, String color);
    void neoRingDoubleCounterClockWise(int circles, int del, String color);
    void neoRingCounterClockWise(int circles, int del, String color);
    void changeColor(uint32_t color);
};

// IMPORTANT: Set pixel PIN, COUNT, and TYPE
// Supported pixel types: WS2812, WS2812B, WS2812B2, WS2811, TM1803, TM1829, SK6812RGBW
#define PIXEL_PIN D2
#define PIXEL_COUNT 16
#define PIXEL_TYPE SK6812RGBW
#define BRIGHTNESS 250 // 0 - 255

Adafruit_NeoPixel _ring = Adafruit_NeoPixel(PIXEL_COUNT, PIXEL_PIN, PIXEL_TYPE);
int p1[] = {0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16};
int p2[] = {8,9,10,11,12,13,14,15,16,0,1,2,3,4,5,6,7};
int gl_red = 0;
int gl_green = 0;
int gl_blue = 0;

// Constructor inits NeoPIXEL-Ring related to pixels (default is pin 2)
/*!
  \param pixels amount of pixels on ring
  \return obj insctance of class
*/
IoTEcoSys_NeoPIXEL_Ring::IoTEcoSys_NeoPIXEL_Ring() {
}

IoTEcoSys_NeoPIXEL_Ring::IoTEcoSys_NeoPIXEL_Ring(int pixel) {
}

//! The init function - nothing to do in this class
/*!
*/
bool IoTEcoSys_NeoPIXEL_Ring::init(){
    Serial.begin(9600);

    _ring.setBrightness(BRIGHTNESS);
    _ring.begin();
    _ring.show(); // Initialize all pixels to 'off'
    return true;
}


// Set RGB
/*!
  \param color red, green, blue, white or off
  \return int err = 0, else = 1
*/
int IoTEcoSys_NeoPIXEL_Ring::setGlRGB( String color) {
  if(color == "red") {
    gl_green = 0;
    gl_red = 255;
    gl_blue = 0;
    //ring.Color(0, 255, 0); // GRB
  }
  else if(color == "green") {
    gl_green = 255;
    gl_red = 0;
    gl_blue = 0;
    //ring.Color(255, 0, 0); // GRB
  }
  else if(color == "blue") {
    gl_green = 0;
    gl_red = 0;
    gl_blue = 255;
    //ring.Color(0, 0, 255);
  }
  else if(color == "white") {
    gl_green = 255;
    gl_red = 255;
    gl_blue = 255;
    _ring.Color(gl_green, gl_red, gl_blue, 255); // GRB+W
  }
  else if(color == "off") {
    gl_green = 0;
    gl_red = 0;
    gl_blue = 0;
    _ring.Color(gl_green, gl_red, gl_blue); // GRB+W
  }
}


// NeoPIXEL ring rotate clockwise
/*!
  \param circles amount of circles to animate
  \param del delay in ms
  \param color red, green, blue, white or off
*/
void IoTEcoSys_NeoPIXEL_Ring::neoRingClockWise(int circles, int del, String color){

    for(int c=0;c<circles;c++){
       setGlRGB("off");
       changeColor(_ring.Color(gl_green, gl_red, gl_blue)); // GRB
       _ring.show();
       for(int i=0;i<16;i++){
          setGlRGB(color);
         _ring.setPixelColor(15-p1[i],_ring.Color(gl_green, gl_red, gl_blue));
          if(i > -1){
            _ring.setPixelColor(15-(p1[i]-1),_ring.Color(0, 0, 0));
          }
          _ring.show();
          delay(del);  
      }
    }
    changeColor(_ring.Color(0, 0, 0)); // GRB
    _ring.show();
}

// NeoPIXEL ring fill clockwise
/*!
  \param showuptime time to showup in ms
  \param del delay in ms
  \param color red, green, blue, white or off
*/
void IoTEcoSys_NeoPIXEL_Ring::neoRingFillClockWise(int showuptime, int del, String color){
    changeColor(_ring.Color(0, 0, 0)); // GRB
    _ring.show();
       for(int i=0;i<16;i++){
          setGlRGB(color);
          _ring.setPixelColor(15-p1[i],_ring.Color(gl_green, gl_red, gl_blue));
          if(i > -1){
          }
          _ring.show();
          delay(del);  
      }
    delay(showuptime);
    changeColor(_ring.Color(0, 0, 0)); // GRB
    _ring.show();
}

// NeoPIXEL ring double counter clockwise
/*!
  \param circles amount of circles to animate
  \param del delay in ms
  \param color red, green, blue, white or off
*/
void IoTEcoSys_NeoPIXEL_Ring::neoRingDoubleCounterClockWise(int circles, int del, String color){

    for(int c=0;c<circles;c++){
       changeColor(_ring.Color(0, 0, 0)); // GRB
       _ring.show();
       for(int i=0;i<16;i++){
          setGlRGB(color);
          _ring.setPixelColor(p1[i],_ring.Color(gl_green, gl_red, gl_blue));
          _ring.setPixelColor(p2[i],_ring.Color(gl_green, gl_red, gl_blue));
          if(i > -1){
            setGlRGB("off");
            _ring.setPixelColor(p1[i]-1,_ring.Color(gl_green, gl_red, gl_blue));
            _ring.setPixelColor(p2[i]-1,_ring.Color(gl_green, gl_red, gl_blue));
          }
          _ring.show();
          delay(del);  
      } 
    }
    changeColor(_ring.Color(0, 0, 0)); // GRB
    _ring.show();
}

// NeoPIXEL ring double clockwise
/*!
  \param circles amount of circles to animate
  \param del delay in ms
  \param color red, green, blue, white or off
*/
void IoTEcoSys_NeoPIXEL_Ring::neoRingCounterClockWise(int circles, int del, String color){

    for(int c=0;c<circles;c++){
       changeColor(_ring.Color(0, 0, 0)); // GRB
       _ring.show();
       for(int i=0;i<16;i++){
          setGlRGB(color); 
          _ring.setPixelColor(p1[i],_ring.Color(gl_green, gl_red, gl_blue));
          if(i > -1){
            setGlRGB("off");
            _ring.setPixelColor(p1[i]-1,_ring.Color(gl_green, gl_red, gl_blue));
          }
          _ring.show();
          delay(del);  
      } 
    }
    changeColor(_ring.Color(0, 0, 0)); // GRB
    _ring.show();
}


// NeoPIXEL ring change color
/*!
  \param color color to change
*/

void IoTEcoSys_NeoPIXEL_Ring::changeColor(uint32_t color) {
  for(uint16_t i=0; i < _ring.numPixels(); i++) {
    _ring.setPixelColor(i, color);
    _ring.show();
  }
}



/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// I.S.I. Stuff goes here:
//

// HX711.DOUT	- pin #A1
// HX711.PD_SCK	- pin #A0

HX711ADC scale(A1, A0);		// parameter "gain" is ommited; the default value 128 is used by the library

double scaleValue = 0.0;    // Reading from ADC


////////////////////////////////////////////////////////////////////
// Diskreter Tiefpass Filter (FIR)
#define N_FILTER 16

const double A[]={0.001,0.005,0.02,0.04,0.07,0.1,0.1,0.1,0.1,0.1,0.1,0.07,0.04,0.02,0.005,0.001};

double filteredValue = 0.0;

double filter(double x) {
    static bool init = false;
    static double w[N_FILTER];
    
    // Initialize w's
    if(false == init) {
        for(int i=0; i<N_FILTER; i++) {
            w[i] = 0.0;
        }
        init = true;
    }
    
    for(int i=N_FILTER-1; i>0; i--) {
        w[i] = w[i-1];
    }
    
    w[0] = x;
    
    double y = 0;
    for(int i=0; i<N_FILTER; i++) {
        y += (A[i] * w[i]);
    }
    
    return y;
}


////////////////////////////////////////////////////////////////////
//  Main Program
double deltaMin = 0.01;
double deltaResume = 0.030;
int timeLimit = 90;
int counter = 0;
int initialized = 0;
String location = "";
IoTEcoSys_NeoPIXEL_Ring ring(16);

////////////////////////////////////////////////////////////////////
//  Serialization
String serialized = "0000000000000000000000000";
void SerializeData() {
    serialized = String::format("%16.5f%8d%d%s", filteredValue, counter, initialized?1:0, location.c_str());
}

////////////////////////////////////////////////////////////////////
//  Initialization via cloud
int Initialize(String argLocation) {
    location = argLocation;
    initialized = true;
    return location.length();
}

void setup() {
  SerializeData();
  Particle.variable("serialized", serialized);
  Particle.variable("weight", scaleValue);
  Particle.variable("avgWeight", filteredValue);
  Particle.variable("counter", counter);
  Particle.variable("location", location);
  Particle.function("init", Initialize);
  scale.set_scale(57000.f); // Original value was 2280. Normalized to kg
  scale.tare();	
  
  // Initialize Ring
  ring.init();  
}

unsigned long seconds = 0;

void loop() {
    if(initialized) {
        // Initialized, run state machines
        static unsigned long lastMillis = millis();
        
        if(millis() - lastMillis >= 500) {
            Particle.syncTime();
            lastMillis = millis();
            
            static int halfSeconds = 0;
            if(++halfSeconds == 2) {
                halfSeconds = 0;
                
                // Every second
                seconds++;
            }
            
            // Every 500ms
            ReadInputs();
        }
        
        // Update statemachines very often
        StateMachineCounter();
        StateMachineInfusion();
    } else {
        // Not initialized, blink light
        uint32_t color;
        
        color = (uint32_t)(255<<16) | (uint32_t)(255<<8) | (uint32_t)(255);
        ring.changeColor(color);
        
        delay(500);
        
        color = 0;
        ring.changeColor(color);
        
        delay(500);
    }
    
    
    SerializeData();
}

////////////////////////////////////////////////////////////////////
//  Input processing
void ReadInputs() {
    scaleValue = scale.get_units(5);
    filteredValue = filter(scaleValue);
    scale.power_down();		// put the ADC in sleep mode
    delay(100);
    scale.power_up();
}

////////////////////////////////////////////////////////////////////
//  State machine "counter"
void StateMachineCounter() {
	// Machine states
	static enum { INIT, COUNT } state = INIT;

	// Active transition (see docs)
	static int transition = 0,
		previousTransition = 0;

	// Global variables

	// Handle states
	switch (state) {
	case INIT:
	{
		if (previousTransition) {
			// Entry action

		}
		else {
			// Do action


			// Check transition conditions
			transition = 1;
		}


		if (transition) {
			// Exit action

		}
	}
	break;

	case COUNT:
	{
		// Local variable
		static unsigned long t;

		if (previousTransition) {
			// Entry action
			t = seconds;
		}
		else {
			// Do action


			// Check transition conditions
			if (seconds - t >= 1) {
				transition = 2;
			}
		}


		if (transition) {
			// Exit action

		}
	}
	break;
	}

#ifdef TESTBENCH
	const char *state2str[] = {
		"INIT",
		"COUNT"
	};
	if (transition) {
		PrintTimestamp();
		std::cout << "StateMachine: Counter, State: " << state2str[state] << ", Transition: " << transition << std::endl;
	}
#endif

	// Handle transition
	switch (transition) {
	case 1:
		// Init -> Count
	{
		// Perform transition action

		// Change state
		state = COUNT;
	}
	break;

	case 2:
		// Count -> Count
	{
		// Perform transition action
		counter++;

		// Change state
		state = COUNT;
	}
	break;
	}

	previousTransition = transition;

	// Deactivate transition
	transition = 0;

}

////////////////////////////////////////////////////////////////////
//  State machine "infusion"
void StateMachineInfusion() {
	// Machine states
	static enum { INIT, RUNNING, STOPPED } state = INIT;

	// Active transition (see docs)
	static int transition = 0,
		previousTransition = 0;

	// Global variables
	static double m;

	// Handle states
	switch (state) {
	case INIT:
	{
		if (previousTransition) {
			// Entry action

		}
		else {
			// Do action


			// Check transition conditions
			transition = 1;
		}


		if (transition) {
			// Exit action

		}
	}
	break;

	case RUNNING:
	{
		// Local variable

		if (previousTransition) {
			// Entry action
			m = filteredValue;
			ChangeColorByTime(0);
		}
		else {
			// Do action


			// Check transition conditions
			if (counter > 30) {
				transition = 3;
			}

			if (filteredValue - m < -1.0 * deltaMin) {
				transition = 2;
			}

			if (filteredValue - m > 0) {
				transition = 6;
			}
		}


		if (transition) {
			// Exit action

		}
	}
	break;

	case STOPPED:
	{
		// Local variables
		static unsigned long t;

		if (previousTransition) {
			// Entry action
			m = filteredValue;
			t = seconds;
		}
		else {
			// Do action
			ChangeColorByTime(seconds - t);

			// Check transition conditions
			static unsigned long t0 = seconds;
			if (seconds - t0 >= 60) {
				// Every 60 seconds
				t0 = seconds;

				if (filteredValue - m < -1.0 * deltaResume) {
					transition = 5;
				}
			}

			if (filteredValue - m > 50) {
				transition = 4;
			}
		}


		if (transition) {
			// Exit action
			counter = 0;
		}
	}
	break;

	}

#ifdef TESTBENCH
	const char *state2str[] = {
		"INIT",
		"RUNNING",
		"STOPPED"
	};
	if (transition) {
		PrintTimestamp();
		std::cout << "StateMachine: Infusion, Counter: " << counter << ", State: " << state2str[state] << ", Transition: " << transition << std::endl;
	}
#endif

	// Handle transition
	switch (transition) {
	case 1:
		// Init -> Running
	{
		// Perform transition action

		// Change state
		state = RUNNING;
	}
	break;

	case 2:
		// Running -> Running
	{
		// Perform transition action
		counter = 0;

		// Change state
		state = RUNNING;
	}
	break;

	case 3:
		// Running -> Stopped
	{
		// Perform transition action

		// Change state
		state = STOPPED;
	}
	break;

	case 4:
		// Stopped -> Running
	{
		// Perform transition action

		// Change state
		state = RUNNING;
	}
	break;

	case 5:
		// Stopped -> Running
	{
		// Perform transition action

		// Change state
		state = RUNNING;
	}
	break;

	case 6:
		// Running -> Running
	{
		// Perform transition action
		counter = 0;

		// Change state
		state = RUNNING;
	}
	break;

	}

	previousTransition = transition;

	// Deactivate transition
	transition = 0;
}

////////////////////////////////////////////////////////////////////
//  Time to color conversion
void ChangeColorByTime(int t) {
    // Calculate color.
    // Starts with green, transforms to red over yellow.
    int red = 0, green = 0, blue = 0;
    int maxTime = timeLimit*3/4; // Full red after this time
    int currentTime = t;
    if(currentTime > maxTime) {
        currentTime = maxTime;
    }
    float f = (float)currentTime / maxTime; // convert to [0 ... 1]
    float a = (/*1.f - */f)*2.f;
    float x = floor(a);
    float y = floor(255.f*(a-x));
    int group = (int)x; // Group: 0->red 1->yellow 2->green
    switch(group)
    {
        case 0:
            red=255;
            green=(int)y;
            blue=0;
            break;
        case 1:
            red=255-(int)y;
            green=255;
            blue=0;
            break;
        case 2:
            red=0;
            green=255;
            blue=(int)y;
            break;
    }
    uint32_t color = (uint32_t)(red<<16) | (uint32_t)(green<<8) | (uint32_t)(blue);
    ring.changeColor(color);
}

#include <stdio.h>

double scaleValue = 0.0;    // Reading from adc (scale)

// Diskreter Tiefpass Filter
#define N_FILTER 16

const double A[]={0.001,0.005,0.02,0.04,0.07,0.1,0.1,0.1,0.1,0.1,0.1,0.07,0.04,0.02,0.005,0.001};

double filteredValue = 0.0;

double filter(double x) {
    static double w[N_FILTER] = {
        0.0,
        0.0,
        0.0,
        0.0,
        0.0,
        0.0,
        0.0,
        0.0,
        0.0,
        0.0,
        0.0,
        0.0,
        0.0,
        0.0,
        0.0,
        0.0
    };
    
    for(int i=N_FILTER-1; i>0; i--) {
        w[i] = w[i-1];
    }
    
    w[0] = x;
    
    double y = 0;
    for(int i=0; i<N_FILTER; i++) {
        y += (A[i] * w[i]);
    }
    
    return y;
}

#include "mex.h"

void mexFunction( int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]){
	int samples = mxGetNumberOfElements(prhs[0]);
    double *x = mxGetData(prhs[0]);
    double *y = malloc(sizeof(double) * samples);
    
    
    for(int i=0; i<samples; i++) {
        y[i] = filter(x[i]);
        mexPrintf("%f => %f\n", x[i], y[i]);
    }
    
    plhs[0] = mxCreateDoubleMatrix(samples, 1, mxREAL);
    memcpy(mxGetPr(plhs[0]), y, samples * sizeof(double));
    free(y);
}