imu.ino

/***************************************************************************
  Example sketch for the MPU9250_WE library

  This sketch shows how to obtain raw accleration data and g values from
  the MPU9250.

  For further information visit my blog:

  https://wolles-elektronikkiste.de/mpu9250-9-achsen-sensormodul-teil-1  (German)
  https://wolles-elektronikkiste.de/en/mpu9250-9-axis-sensor-module-part-1  (English)

***************************************************************************/

/*

  Cant decide which data to use, ResultantG seems to give data on tilt, accCorrRaw_z doesnt.

  I will get the raw data regardless and map it to 0-255
  I will get average raw value in the beginning 3 seconds to know the still values.
  I will be reading and storing max and min data's on the way going so I can adjust the swing rate .
  Then I will display.

  After successful beat display I will do the visual adjustments called Gamma Correction:  https://www.reddit.com/r/FastLED/comments/s40obo/gamma_correction_for_fastled_beatsin8/

  Priorities - ToDo List

  ###)pacifica_loop on stable state.

   ###) Change rainbow to rainbow_HSV

  ###) Data smoothing.
  -
  ###) Gamma Correction



  ###) Wireless communication

  ###) Mounting

*/




#include <MPU9250_WE.h>
#include <Wire.h>

#include "FastLED.h"                                          // FastLED library.
#include <Smoothed.h>
#define MPU9250_ADDR 0x68

/* There are several ways to create your MPU9250 object:
   MPU9250_WE myMPU9250 = MPU9250_WE()              -> uses Wire / I2C Address = 0x68
   MPU9250_WE myMPU9250 = MPU9250_WE(MPU9250_ADDR)  -> uses Wire / MPU9250_ADDR
   MPU9250_WE myMPU9250 = MPU9250_WE(&wire2)        -> uses the TwoWire object wire2 / MPU9250_ADDR
   MPU9250_WE myMPU9250 = MPU9250_WE(&wire2, MPU9250_ADDR) -> all together
   Successfully tested with two I2C busses on an ESP32
*/

////////////////////////////////////////////////FASTLED


uint8_t thishue = 0;                                          // Starting hue value.
uint8_t deltahue = 10;

#define LED_DT 13                                             // Data pin to connect to the strip.
#define LED_TYPE WS2812B                                       // What kind of strip are you using (WS2801, WS2812B or APA102)?
#define COLOR_ORDER GRB                                       // It's GRB for WS2812B and BGR for APA102

#define NUM_LEDS 325                                           // Number of LED's.
#define MAX_BRIGHTNESS 255      // Thats full on, watch the power!
#define MIN_BRIGHTNESS 0        // set to a minimum of 25%
struct CRGB leds[NUM_LEDS];                                   // Initialize our LED array.

int samples = 0;
const int numReadings = 30;

int readings[numReadings];      // the readings from the analog input
int readIndex = 0;              // the index of the current reading
float total = 0;                  // the running total
float average_RAWDATA = 0;                // the average

float maximum_RAWDATA;
float minimum_RAWDATA;

unsigned long startMillis;
unsigned long currentMillis;
const unsigned long period = 5000;


unsigned long startPacificaTime;
unsigned long currentPacificaMillis;
const unsigned long PacificaStartperiod = 3000;
boolean MeasurementAlreadyTaken = false;

const unsigned long checkingperiod = 3000;
boolean ranonce = false;
boolean ranonce2 = false;


float R;

Smoothed <float> mySensor;

extern const uint8_t gamma8[];


typedef void (*SimplePatternList[])();
uint8_t gCurrentPatternNumber = 0; // Index number of which pattern is current

static unsigned long timestamp = 0;
unsigned long timeNow = millis();
////////////////////////////////////////////////FASTLED/////////////////////////////////////////////////





MPU9250_WE myMPU9250 = MPU9250_WE(MPU9250_ADDR);


///////**************************************************************SETUP******************************************************//////////////////////////////


void setup() {
  Serial.begin(230400);

  ////FASTLED
  Wire.setClock(400000),
                ////FASTLED

                Wire.begin();
  if (!myMPU9250.init()) {
    Serial.println("MPU9250 does not respond");
  }
  else {
    Serial.println("MPU9250 is connected");
  }


  ////////////////////////////////////////////////FASTLED
  LEDS.addLeds<LED_TYPE, LED_DT, COLOR_ORDER>(leds, NUM_LEDS);        // Use this for WS2812B
  FastLED.setBrightness(MIN_BRIGHTNESS);

  for (int thisReading = 0; thisReading < numReadings; thisReading++) {
    readings[thisReading] = 0;
  }

  R = (255 * log10(2)) / (log10(255));

  mySensor.begin(SMOOTHED_AVERAGE, 3); // ????


  ////////////////////////////////////////////////FASTLED/////////////////////////////////////////////////


  /* The slope of the curve of acceleration vs measured values fits quite well to the theoretical
     values, e.g. 16384 units/g in the +/- 2g range. But the starting point, if you position the
     MPU9250 flat, is not necessarily 0g/0g/1g for x/y/z. The autoOffset function measures offset
     values. It assumes your MPU9250 is positioned flat with its x,y-plane. The more you deviate
     from this, the less accurate will be your results.
     The function also measures the offset of the gyroscope data. The gyroscope offset does not
     depend on the positioning.
     This function needs to be called at the beginning since it can overwrite your settings!
  */
  Serial.println("Position you MPU9250 flat and don't move it - calibrating...");
  delay(1000);
  myMPU9250.autoOffsets();
  Serial.println("Done!");

  /*  This is a more accurate method for calibration. You have to determine the minimum and maximum
      raw acceleration values of the axes determined in the range +/- 2 g.
      You call the function as follows: setAccOffsets(xMin,xMax,yMin,yMax,zMin,zMax);
      Use either autoOffset or setAccOffsets, not both.
  */
  //myMPU9250.setAccOffsets(-14240.0, 18220.0, -17280.0, 15590.0, -20930.0, 12080.0);

  /*  Sample rate divider divides the output rate of the gyroscope and accelerometer.
      Sample rate = Internal sample rate / (1 + divider)
      It can only be applied if the corresponding DLPF is enabled and 0<DLPF<7!
      Divider is a number 0...255
  */
  myMPU9250.setSampleRateDivider(1);

  /*  MPU9250_ACC_RANGE_2G      2 g
      MPU9250_ACC_RANGE_4G      4 g
      MPU9250_ACC_RANGE_8G      8 g
      MPU9250_ACC_RANGE_16G    16 g
  */
  myMPU9250.setAccRange(MPU9250_ACC_RANGE_2G);

  /*  Enable/disable the digital low pass filter for the accelerometer
      If disabled the bandwidth is 1.13 kHz, delay is 0.75 ms, output rate is 4 kHz
  */
  myMPU9250.enableAccDLPF(false);

  /*  Digital low pass filter (DLPF) for the accelerometer, if enabled
      MPU9250_DPLF_0, MPU9250_DPLF_2, ...... MPU9250_DPLF_7
       DLPF     Bandwidth [Hz]      Delay [ms]    Output rate [kHz]
         0           460               1.94           1
         1           184               5.80           1
         2            92               7.80           1
         3            41              11.80           1
         4            20              19.80           1
         5            10              35.70           1
         6             5              66.96           1
         7           460               1.94           1
  */
  myMPU9250.setAccDLPF(MPU9250_DLPF_1);

  /*  Set accelerometer output data rate in low power mode (cycle enabled)
       MPU9250_LP_ACC_ODR_0_24          0.24 Hz
       MPU9250_LP_ACC_ODR_0_49          0.49 Hz
       MPU9250_LP_ACC_ODR_0_98          0.98 Hz
       MPU9250_LP_ACC_ODR_1_95          1.95 Hz
       MPU9250_LP_ACC_ODR_3_91          3.91 Hz
       MPU9250_LP_ACC_ODR_7_81          7.81 Hz
       MPU9250_LP_ACC_ODR_15_63        15.63 Hz
       MPU9250_LP_ACC_ODR_31_25        31.25 Hz
       MPU9250_LP_ACC_ODR_62_5         62.5 Hz
       MPU9250_LP_ACC_ODR_125         125 Hz
       MPU9250_LP_ACC_ODR_250         250 Hz
       MPU9250_LP_ACC_ODR_500         500 Hz
  */
  //myMPU9250.setLowPowerAccDataRate(MPU9250_LP_ACC_ODR_500);

  /* sleep() sends the MPU9250 to sleep or wakes it up.
     Please note that the gyroscope needs 35 milliseconds to wake up.
  */
  //myMPU9250.sleep(true);

  /* If cycle is set, and standby or sleep are not set, the module will cycle between
       sleep and taking a sample at a rate determined by setLowPowerAccDataRate().
  */
  //myMPU9250.enableCycle(true);

  /* You can enable or disable the axes for gyroscope and/or accelerometer measurements.
     By default all axes are enabled. Parameters are:
     MPU9250_ENABLE_XYZ  //all axes are enabled (default)
     MPU9250_ENABLE_XY0  // X, Y enabled, Z disabled
     MPU9250_ENABLE_X0Z
     MPU9250_ENABLE_X00
     MPU9250_ENABLE_0YZ
     MPU9250_ENABLE_0Y0
     MPU9250_ENABLE_00Z
     MPU9250_ENABLE_000  // all axes disabled
  */
  //myMPU9250.enableAccAxes(MPU9250_ENABLE_XYZ);

}

///////**********************************************************SETUP***********************************************************


SimplePatternList gPatterns = {
  rainbow_march, //0 //
  pacifica_loop, //1
};


///////****************************************************************************************************************/LOOP******************************************************//////////////////////////////
void loop() {
  xyzFloat accRaw = myMPU9250.getAccRawValues();
  xyzFloat accCorrRaw = myMPU9250.getCorrectedAccRawValues();
  xyzFloat gValue = myMPU9250.getGValues();
  float resultantG = myMPU9250.getResultantG(gValue);

  //Corrected ('calibrated') acceleration values
  /*
    Serial.print("\t accCorrRaw.x:  \t");   Serial.print(accCorrRaw.x);
    Serial.print("\t acceleration value y:  \t");   Serial.print(accCorrRaw.y);
    Serial.print("\t acceleration value z: \t");   Serial.print(accCorrRaw.z);

  */
  mySensor.add(accCorrRaw.z);
  float smoothedSensorValueAvg = mySensor.get();
  /*
    Serial.print("\t smoothedSensorValueAvg:  \t");   Serial.print(smoothedSensorValueAvg); //

    Serial.print("\t g value x: \t");   Serial.print(gValue.x);
    Serial.print("\t g value y: \t");   Serial.print(gValue.y);
    Serial.print("\t g value z: \t");  Serial.print(gValue.z);

    // should always be 1 g if only gravity acts on the sensor.
    Serial.print("\tResultant g: \t ");  Serial.print(resultantG, 6);


  */



  ////////////////////////////////////////////////FASTLED


  if (ranonce == false)///Reading calibration first five seconds. I set the ranonce to True from False and start the millis timer. Ranonce will run until ranonce is false.
  {
    startMillis = millis();
    ranonce = true;
  }


  currentMillis = millis();  //get the current "time" and set it to currentmillis(actually the number of milliseconds since the program started)

  //test whether we are within average reading period of 3 seconds.
  //The value of swing being still. I get the average of 50 values:

  if ( currentMillis - startMillis >= 3000 && currentMillis - startMillis <= period) //after 3 seconds and before the 3 second measuring period measure the stable swing.

  {

    total = total - readings[readIndex];   // subtract the last reading:
    readings[readIndex] = accCorrRaw.z; // read from the sensor. We will get an average of our beginning value to know our beginning value clearly.:
    total = total + readings[readIndex]; // add the reading to the total:
    readIndex = readIndex + 1; // advance to the next position in the array:
    if (readIndex >= numReadings) { // if we're at the end of the array...
      readIndex = 0; // ...wrap around to the beginning:
    }
    average_RAWDATA = total / numReadings;   // calculate the average and call it our average_rawdata:

    /*
      Serial.print(" \t ReadIndex: \t"); Serial.print(readIndex);  //
      Serial.print("\t AccCorrRaw.z: \t"); Serial.print(accCorrRaw.z);  //
      Serial.print("\t Total: \t"); Serial.print(total);  //
      Serial.print("\t Average_RAWDATA: \t"); Serial.print(average_RAWDATA);  //
    */
  }
  else if (currentMillis - startMillis > period) { // ????? COMMENT
    if (ranonce2 == false)//
    {
      maximum_RAWDATA = average_RAWDATA;
      minimum_RAWDATA = average_RAWDATA;
      ranonce2 = true;
    }

    if (accCorrRaw.z < minimum_RAWDATA) { // determine minimum value
      minimum_RAWDATA = accCorrRaw.z;
    }
    if (accCorrRaw.z > maximum_RAWDATA) { //determine maximum value
      maximum_RAWDATA = accCorrRaw.z;


    }

  }

  EVERY_N_MILLISECONDS(6000) {
    minimum_RAWDATA = average_RAWDATA; // every 6 seconds reset the minimum raw data to average raw data from the beginning. WHYYYY?????
  }

  /*EVERY_N_MILLISECONDS(10000){
     maximum_RAWDATA = average_RAWDATA;
      }*/

  //I am just getting the minimum and maximum data and converting them to 0-255.
  //Somehow the maximum data was always the swing, regardless of direction. and minimum data was no movement.
  //Except when the swing started, that value of nonmovement was a number inbetween for some reason.
  double mappedRAW = map(smoothedSensorValueAvg, minimum_RAWDATA, maximum_RAWDATA, 0.0, 255.0);
  int BrightnessValue = abs(mappedRAW);

  //  int BrightnessValue = abs(pow(2.0, (mappedRAW / R)) - 1); //Calculates the value 2 raised to (absolutegyroY/5). Pow() //pow(2,absolutegyroY/5); https://diarmuid.ie/blog/pwm-exponential-led-fading-on-arduino-or-other-platforms


  uint8_t lookupBrightness = pgm_read_byte(&gamma8[BrightnessValue]);



  /*
    EVERY_N_MILLISECONDS(5) {                           // FastLED based non-blocking routine to update/display the sequence.
     gPatterns[1]();
    }
  */



  // Gamma Like Correction.
  //use a look up table to adjust the input brightness





  ///// I start the timer
  ///// If 3 seconds has passed since brightnessvalue is smaller than 20 I set the pattern to pacifica
  ///// If not, the patter is set to rainbow
  /////

  /*

      currentPacificaMillis = millis();

      if (BrightnessValue <= 20 && MeasurementAlreadyTaken == false) //// Brightness is smaller than 20 we start the timer so that after 3 seconds we can switch to Pacifica
      {
        startPacificaTime = millis();
        MeasurementAlreadyTaken == true;
      }
      else if (BrightnessValue > 20)
      {
        startPacificaTime = millis();;
        MeasurementAlreadyTaken == false;
      }



      if ((currentPacificaMillis - startPacificaTime) >= PacificaStartperiod) //// We switch to Pacifica if the pacifica millis is larger than 3 seconds
      {

        FastLED.setBrightness(255);
        gCurrentPatternNumber = 1;
      }
      else {
        FastLED.setBrightness(lookupBrightness);
        gCurrentPatternNumber = 0;
      }

         gPatterns[gCurrentPatternNumber]();
  */

  static unsigned long timestamp = 0;
  unsigned long timeNow = millis();

  if (BrightnessValue > 25)
  {
    // Reset timestamp since motion detected
    timestamp = timeNow;

    // Do code B
    gPatterns[0](); //rainbow_march
    FastLED.setBrightness(lookupBrightness);
  FastLED.show();

  }
  else if (BrightnessValue < 25 && timeNow - timestamp > 3000)
  {
    // Do code A
    gPatterns[1](); //pacifica_loop
    FastLED.setBrightness(255);
  FastLED.show();
    // Reset timestamp
    timestamp = timeNow;
  }




  /*
    Serial.print("\t accCorrRaw.z:  \t"); Serial.print(accCorrRaw.z);  //
    Serial.print("\t average_RAWDATA:  \t"); Serial.print(average_RAWDATA);  //
    Serial.print("\t maximum_RAWDATA: \t "); Serial.print(maximum_RAWDATA);  //
    Serial.print("\t minimum_RAWDATA: \t "); Serial.print(minimum_RAWDATA);  //
    Serial.print("\t mappedRAW: \t "); Serial.print(mappedRAW);  //

  */

  Serial.print("\t BrightnessValue:  \t"); Serial.println(BrightnessValue);  //
  //  delay(100);

  ////////////////////////////////////////////////FASTLED/////////////////////////////////////////////////



}
//////////////////////////////LOOP//////////////////////////////////











/// Will try to switch this to fill_rainbow_hsv.
void rainbow_march() {                                        // The fill_rainbow call doesn't support brightness levels

  fill_rainbow(leds, NUM_LEDS, thishue, deltahue);
  //    (300);// Use FastLED's fill_rainbow routine.
} // rainbow_march()


const uint8_t PROGMEM gamma8[] = {
  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,  1,  1,  1,  1,
  1,  1,  1,  1,  1,  1,  1,  1,  1,  2,  2,  2,  2,  2,  2,  2,
  2,  3,  3,  3,  3,  3,  3,  3,  4,  4,  4,  4,  4,  5,  5,  5,
  5,  6,  6,  6,  6,  7,  7,  7,  7,  8,  8,  8,  9,  9,  9, 10,
  10, 10, 11, 11, 11, 12, 12, 13, 13, 13, 14, 14, 15, 15, 16, 16,
  17, 17, 18, 18, 19, 19, 20, 20, 21, 21, 22, 22, 23, 24, 24, 25,
  25, 26, 27, 27, 28, 29, 29, 30, 31, 32, 32, 33, 34, 35, 35, 36,
  37, 38, 39, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 50,
  51, 52, 54, 55, 56, 57, 58, 59, 60, 61, 62, 63, 64, 66, 67, 68,
  69, 70, 72, 73, 74, 75, 77, 78, 79, 81, 82, 83, 85, 86, 87, 89,
  90, 92, 93, 95, 96, 98, 99, 101, 102, 104, 105, 107, 109, 110, 112, 114,
  115, 117, 119, 120, 122, 124, 126, 127, 129, 131, 133, 135, 137, 138, 140, 142,
  144, 146, 148, 150, 152, 154, 156, 158, 160, 162, 164, 167, 169, 171, 173, 175,
  177, 180, 182, 184, 186, 189, 191, 193, 196, 198, 200, 203, 205, 208, 210, 213,
  215, 218, 220, 223, 225, 228, 231, 233, 236, 239, 241, 244, 247, 249, 252, 255
};


//---------------------------------------------------------------
// custom fill_rainbow function that takes h,s,v input
void fill_rainbow_hsv( struct CRGB * pFirstLED, int numToFill,
                       uint8_t initialhue, uint8_t initialsat,
                       uint8_t initialval, uint8_t deltahue )
{
  CHSV hsv;
  hsv.hue = initialhue;
  hsv.sat = initialsat;
  hsv.val = initialval;
  for ( int i = 0; i < numToFill; i++) {
    pFirstLED[i] = hsv;
    hsv.hue += deltahue;
  }
}



//////////////////////////////////////////////////////PACIFICA

CRGBPalette16 pacifica_palette_1 =
{ 0x000507, 0x000409, 0x00030B, 0x00030D, 0x000210, 0x000212, 0x000114, 0x000117,
  0x000019, 0x00001C, 0x000026, 0x000031, 0x00003B, 0x000046, 0x14554B, 0x28AA50
};
CRGBPalette16 pacifica_palette_2 =
{ 0x000507, 0x000409, 0x00030B, 0x00030D, 0x000210, 0x000212, 0x000114, 0x000117,
  0x000019, 0x00001C, 0x000026, 0x000031, 0x00003B, 0x000046, 0x0C5F52, 0x19BE5F
};
CRGBPalette16 pacifica_palette_3 =
{ 0x000208, 0x00030E, 0x000514, 0x00061A, 0x000820, 0x000927, 0x000B2D, 0x000C33,
  0x000E39, 0x001040, 0x001450, 0x001860, 0x001C70, 0x002080, 0x1040BF, 0x2060FF
};


void pacifica_loop()
{
  // Increment the four "color index start" counters, one for each wave layer.
  // Each is incremented at a different speed, and the speeds vary over time.
  static uint16_t sCIStart1, sCIStart2, sCIStart3, sCIStart4;
  static uint32_t sLastms = 0;
  uint32_t ms = GET_MILLIS();
  uint32_t deltams = ms - sLastms;
  sLastms = ms;
  uint16_t speedfactor1 = beatsin16(3, 179, 269);
  uint16_t speedfactor2 = beatsin16(4, 179, 269);
  uint32_t deltams1 = (deltams * speedfactor1) / 256;
  uint32_t deltams2 = (deltams * speedfactor2) / 256;
  uint32_t deltams21 = (deltams1 + deltams2) / 2;
  sCIStart1 += (deltams1 * beatsin88(1011, 10, 13));
  sCIStart2 -= (deltams21 * beatsin88(777, 8, 11));
  sCIStart3 -= (deltams1 * beatsin88(501, 5, 7));
  sCIStart4 -= (deltams2 * beatsin88(257, 4, 6));

  // Clear out the LED array to a dim background blue-green
  fill_solid( leds, NUM_LEDS, CRGB( 2, 6, 10));

  // Render each of four layers, with different scales and speeds, that vary over time
  pacifica_one_layer( pacifica_palette_1, sCIStart1, beatsin16( 3, 11 * 256, 14 * 256), beatsin8( 10, 70, 130), 0 - beat16( 301) );
  pacifica_one_layer( pacifica_palette_2, sCIStart2, beatsin16( 4,  6 * 256,  9 * 256), beatsin8( 17, 40,  80), beat16( 401) );
  pacifica_one_layer( pacifica_palette_3, sCIStart3, 6 * 256, beatsin8( 9, 10, 38), 0 - beat16(503));
  pacifica_one_layer( pacifica_palette_3, sCIStart4, 5 * 256, beatsin8( 8, 10, 28), beat16(601));

  // Add brighter 'whitecaps' where the waves lines up more
  pacifica_add_whitecaps();

  // Deepen the blues and greens a bit
  pacifica_deepen_colors();
}

// Add one layer of waves into the led array
void pacifica_one_layer( CRGBPalette16& p, uint16_t cistart, uint16_t wavescale, uint8_t bri, uint16_t ioff)
{
  uint16_t ci = cistart;
  uint16_t waveangle = ioff;
  uint16_t wavescale_half = (wavescale / 2) + 20;
  for ( uint16_t i = 0; i < NUM_LEDS; i++) {
    waveangle += 250;
    uint16_t s16 = sin16( waveangle ) + 32768;
    uint16_t cs = scale16( s16 , wavescale_half ) + wavescale_half;
    ci += cs;
    uint16_t sindex16 = sin16( ci) + 32768;
    uint8_t sindex8 = scale16( sindex16, 240);
    CRGB c = ColorFromPalette( p, sindex8, bri, LINEARBLEND);
    leds[i] += c;
  }
}

// Add extra 'white' to areas where the four layers of light have lined up brightly
void pacifica_add_whitecaps()
{
  uint8_t basethreshold = beatsin8( 9, 55, 65);
  uint8_t wave = beat8( 7 );

  for ( uint16_t i = 0; i < NUM_LEDS; i++) {
    uint8_t threshold = scale8( sin8( wave), 20) + basethreshold;
    wave += 7;
    uint8_t l = leds[i].getAverageLight();
    if ( l > threshold) {
      uint8_t overage = l - threshold;
      uint8_t overage2 = qadd8( overage, overage);
      leds[i] += CRGB( overage, overage2, qadd8( overage2, overage2));
    }
  }
}

// Deepen the blues and greens
void pacifica_deepen_colors()
{
  for ( uint16_t i = 0; i < NUM_LEDS; i++) {
    leds[i].blue = scale8( leds[i].blue,  145);
    leds[i].green = scale8( leds[i].green, 200);
    leds[i] |= CRGB( 2, 5, 7);
  }
}




////////////////////////////////////PACIFICA////////////////////////////////////////////////////////