The Posture Practice and Trainer

#include <CurieIMU.h>
#include <MadgwickAHRS.h>
#include <EEPROM.h>

Madgwick filter;
unsigned long microsPerReading, microsPrevious;
float accelScale, gyroScale;
float front_threshold = 0.5, back_threshold = -0.5, right_side_threshold=0.5, left_side_threshold= -0.5; 
unsigned short buttonPresses = 0;
boolean LearnMode = false;

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

  // start the IMU and filter
  CurieIMU.begin();
  CurieIMU.setGyroRate(25);
  CurieIMU.setAccelerometerRate(25);
  filter.begin(25);

  // Set the accelerometer range to 2G
  CurieIMU.setAccelerometerRange(2);
  // Set the gyroscope range to 250 degrees/second
  CurieIMU.setGyroRange(250);

  // initialize variables to pace updates to correct rate
  microsPerReading = 1000000 / 25;
  microsPrevious = micros();

  pinMode(2, OUTPUT);
  pinMode(3, INPUT);

  //  EEPROM_writeDouble(0x03,left_side_threshold);
  //  EEPROM_writeDouble(0x02,right_side_threshold);
  //  EEPROM_writeDouble(0x01,back_threshold);
  //  EEPROM_writeDouble(0x00,front_threshold);
  //
  //  Check if thresholds have been previously saved
  //  NOT READY YET
  //
  //  if(EEPROM_readDouble(0x00)!=0)
  //    front_threshold=EEPROM_readDouble(0x00);
  //  if(EEPROM_readDouble(0x01)!=0)
  //    back_threshold=EEPROM_readDouble(0x01);
  //  if(EEPROM_readDouble(0x02)!=0)
  //    right_side_threshold=EEPROM_readDouble(0x02);
  //  if(EEPROM_readDouble(0x03)!=0)
  //    left_side_threshold=EEPROM_readDouble(0x03);
}

void loop() {
  int aix, aiy, aiz;
  int gix, giy, giz;
  float ax, ay, az;
  float gx, gy, gz;
  float roll, pitch, heading;
  unsigned long microsNow;

  //
  buttonPresses += digitalRead(3);
  Serial.print(buttonPresses);
  Serial.print(" ");
  if(buttonPresses == 15)
  {
    //
    if(LearnMode)
      LearnMode=false;
    else
      LearnMode=true;
    buttonPresses = 0;
  }
  
  //  Reset to default thresholds
  //  NOT READY YET
  if(buttonPresses == 275)
  {
    left_side_threshold = 0.5;
    //EEPROM_writeDouble(0x03,left_side_threshold);
    right_side_threshold = 0.5;
    //EEPROM_writeDouble(0x02,right_side_threshold);
    back_threshold = 0.5;
    //EEPROM_writeDouble(0x01,back_threshold);
    front_threshold = 0.5;
    //EEPROM_writeDouble(0x00,front_threshold);
  }
  
  // check if it's time to read data and update the filter
  microsNow = micros();
  if (microsNow - microsPrevious >= microsPerReading) {

    // read raw data from CurieIMU
    CurieIMU.readMotionSensor(aix, aiy, aiz, gix, giy, giz);

    // convert from raw data to gravity and degrees/second units
    ax = convertRawAcceleration(aix);
    ay = convertRawAcceleration(aiy);
    az = convertRawAcceleration(aiz);
    gx = convertRawGyro(gix);
    gy = convertRawGyro(giy);
    gz = convertRawGyro(giz);

    // update the filter, which computes orientation
    filter.updateIMU(gx, gy, gz, ax, ay, az);

    //
    if(LearnMode)
    { //
      if(ay<0 && ay<left_side_threshold)
      { left_side_threshold = ay;
        //EEPROM_writeDouble(0x03,left_side_threshold);
      }
      if(ay>=0 && ay>right_side_threshold)
      { right_side_threshold = ay;
        //EEPROM_writeDouble(0x02,right_side_threshold);
      }
      if(az<0 && az<back_threshold)
      { back_threshold = az;
        //EEPROM_writeDouble(0x01,back_threshold);
      }
      if(az>=0 && az>front_threshold)
      { front_threshold = az;
        //EEPROM_writeDouble(0x00,front_threshold);
      }
    }
    else
    { if(ay<left_side_threshold || ay>right_side_threshold || az<back_threshold || az>front_threshold)
      { 
        digitalWrite(2, HIGH);
      }
      else
      {
        digitalWrite(2, LOW);
      }
    }
    
    delay(50);

    // print the heading, pitch and roll
    roll = filter.getRoll();
    pitch = filter.getPitch();
    heading = filter.getYaw();

//    Serial.print(left_side_threshold);
//    Serial.print(" ");
//    Serial.print(right_side_threshold);
//    Serial.print(" ");
//    Serial.print(back_threshold);
//    Serial.print(" ");
//    Serial.println(front_threshold);

//    Serial.print("Orientation: ");
//    Serial.print(heading);
//    Serial.print(" ");
//    Serial.print(pitch);
//    Serial.print(" ");
//    Serial.print(roll);
//
    Serial.print(" ");
    Serial.print(ax);
    Serial.print(" ");
    Serial.print(ay);
    Serial.print(" ");
    Serial.println(az);
//    Serial.print(" ");
//    Serial.println(EEPROM_readDouble(0x00));

    // increment previous time, so we keep proper pace
    microsPrevious = microsPrevious + microsPerReading;
  }
}

float convertRawAcceleration(int aRaw) {
  // since we are using 2G range
  // -2g maps to a raw value of -32768
  // +2g maps to a raw value of 32767
  
  float a = (aRaw * 2.0) / 32768.0;
  return a;
}

float convertRawGyro(int gRaw) {
  // since we are using 250 degrees/seconds range
  // -250 maps to a raw value of -32768
  // +250 maps to a raw value of 32767
  
  float g = (gRaw * 250.0) / 32768.0;
  return g;
}
// ************************************************
// Write floating point values to EEPROM
// ************************************************
void EEPROM_writeDouble(int address, double value)
{
  byte* p = (byte*)(void*)&value;
  for (int i = 0; i < sizeof(value); i++)
  {
    EEPROM.write(address++, *p++);
  }
}
// ************************************************
// Read floating point values from EEPROM
// ************************************************
double EEPROM_readDouble(int address)
{
  double value = 0.0;
  byte* p = (byte*)(void*)&value;
  for (int i = 0; i < sizeof(value); i++)
  {
    *p++ = EEPROM.read(address++);
  }
  return value;
}