//Wheelieometer
//=============
//Use an accelerometer to show instantaneous angle, max angle and time >15 degrees
//V.13 With averaging of measured angle
//This copy has been tidied up to demonstrate the concept and Arduino programming

//Mount the GY-291 accelerometer board with the connection pins at front of bike parallel with bars and components uppermost.

//External libraries to be included in the compilation - text within angle brackets < > is the filename
#include <Wire.h> //I2C Interface support, for the GY-291 accelerometer.  Pins are wired (GY-291 to Arduino Nano or Uno): VCC and CS to 5v, Gnd to Gnd & SDO, A4 to SDA, A5 to SCL.  Setting CS high enables I2C mode and SDO low sets default I2C address
#include <SSD1306Ascii.h> //OLED Display library supporting text only and not requiring a data buffer
#include <SSD1306AsciiWire.h> //OLED Display library extension to support I2C communication 

//Defined constants, the name used in the code followed by the value that will replace it at compile time
#define I2COLedAdd 0x3C //I2C address for OLED display - this may vary between suppliers.  Search the internet for I2C Scanner to find sketches that report the addresses of all connected devices
#define I2CAccAdd 0x53 //I2C address for accelerometer  - this may vary between suppliers.  Search the internet for I2C Scanner to find sketches that report the addresses of all connected devices
#define GRangeAdd 0x31 //Address at which the measurement (g) range is stored on the accelerometer
#define GRangeSet 0x01 //Value of measurement range parameter for a range of +/- 4g
#define PowerSaveAdd 0x2D //Address at which the power save mode is stored on the accelerometer
#define PowerSaveMode 0x08 //Value of power save parameter required for measurement mode
#define XStartAdd 0x32 //Address at which the measured  X value starts on the accelerometer (X value ios 2 bytes, and is followed by the Y and Z values)
#define PinLEDG 6 //Arduino digital pin connected to the anode of the green LED anode, this LED indicates a new maximum time has been achieved
#define PinLEDY 7 //Arduino digital pin connected to the anode of the Yellow LED anode, this LED indicates the minimum angle is currently exceeded (wheelie in progress)
#define DisplayUDInt 250 //Interval at which the display is updated, in milliseconds (ms)
#define MinAngle 15.0 //Minimum angle required to detect a wheelie, must be a floating point value, not an integer
#define MinTime 500 //Minimum wheelie time to elapse before wiping previous attempt, in ms
#define AveragingTime 250 //Angle measurement averaging time in ms, smooths the measured angle to reduce effect of vibrations

//Global variables, the value of these parameters is available anywhere within the sketch (program)
//Angle measurements
double AngleDatum = 0;  //Datum angle to be subtracted from measured angle.  This is measured just after switch-on, and accounts for non-horizontal mounting of the wheelieometer
double Angle; //Measured angle in degrees, positive value indicates front of bike is raised
double SumAngle; //Sum of measured angles over the averaging period
unsigned long NMeasurements; //Number of measurements taken during the averaging period
double AverageAngle; //Average angle measured, calculated at the end of the averaging period, as SumAngle / NMeasurements
unsigned long AverageDatum; //Internal timer value at start of averaging period
double MaxAngle; //Maximum angle recorded since switch on or reset

//Wheelie timer
unsigned long StartTime; //Internal timer value at start of current wheelie attempt
byte TimerStarted = 0;  //1 if a wheelie is currently in progress, 0 if not
unsigned long TotalTime = 0; //Current duration of current wheelie attempt
unsigned long MaxTime; //Maximum duration of wheelie achieved since switch on or reset

//Display update timer
unsigned long LastDispUD; //Internal timer value when display last updated

//Initialisation
byte Calibrated = 0; //Set to 1 when initial calibration of angle has been completed to account for mounting angle of device on bike

SSD1306AsciiWire oled; //Create reference to OLED display object

//Setup function: This is executed once on startup  
void setup() 
{
  //Initialise I2C communications
  Wire.begin(); 

  //Set measurement range of accelerometer sensor
  Wire.beginTransmission(I2CAccAdd); //Open device, address with SDO pin grounded
  Wire.write(GRangeAdd); //Address of g range of sensor
  Wire.write(GRangeSet); //Set g range to +/-4g
  Wire.endTransmission();

  //Set measurement mode of accelerometer sensor
  Wire.beginTransmission(I2CAccAdd); 
  Wire.write(PowerSaveAdd); //Power Save address
  Wire.write(PowerSaveMode); //Set to measurement mode
  Wire.endTransmission();

  //Initialise LEDs
  pinMode(PinLEDG, OUTPUT); //Set pin mode be an output
  pinMode(PinLEDY, OUTPUT); //Set pin mode be an output
  digitalWrite(PinLEDG, LOW); //Turn LED off
  digitalWrite(PinLEDY, LOW); //Turn LED off

  //Initialise Display
  oled.begin(&Adafruit128x64, I2COLedAdd); // set the OLED display's number of columns and rows
  oled.setFont(System5x7); //Set font
  oled.set2X(); //Double the font size.  With the defined font, each character is 2 rows high and 10 characters will fit on a line

  //Initialise angle measurement averaging parameters
  AverageDatum = millis(); //Store timer value at start of averaging period
  SumAngle = 0.0; //Zero the sum of measured angles
  NMeasurements = 0;  //Zero the number of angles measured
}

//Main loop, this repeats until device is turned off or reset (by connecting the RST pin to Gnd)
void loop() {
  //Read the measurements from the accelerometer
  byte _buff[6]; //Define array to receive data
  int i = 0; //Counter to loop over bytes of data as they are read
  //float alpha = 0.5; //???
  Wire.beginTransmission(I2CAccAdd); 
  Wire.write(XStartAdd); //X value start address (there are 2 bytes for X, followed by 2 for Y and 2 for Z = 6 bytes in consecutive addresses 0x32 to 0x37
  Wire.endTransmission();
  Wire.beginTransmission(I2CAccAdd);
  Wire.requestFrom(I2CAccAdd,6); //Read 6 bytes to buffer: X, Y, Z values, 2 bytes each
  while(Wire.available())
  {
    _buff[i] = Wire.read(); //Read a byte from input buffer to array
    i++; 
  }
  Wire.endTransmission();

  //Copy values from array to variables, combining the two bytes that make up each value
  //float x = (((int)_buff[1]) << 8) | _buff[0]; //x not used in this version
  float y = (((int)_buff[3]) << 8) | _buff[2];
  float z = (((int)_buff[5]) << 8) | _buff[4];

  //Scale values: multiply by constant
  //x *= 0.0078; //x not used in this version
  y *= 0.0078;
  z *= 0.0078;
  
  //Calculate angle and convert from radians to degrees
  Angle = (atan2(-y, z) * 180.0) / M_PI;
  //Pitch = (atan2(x, sqrt(y * y + z * z)) * 180.0) / M_PI; //This is the angle sideways from vertical and isn't used in this version (fully developed version has a tilt detection option)

  //If not calibrated, do initial one-off actions, else do normal measurements and display updating
  if (!Calibrated)
  {
    //Initial calibration to account for mounting angle
    AngleDatum = Angle; //Store first measured angle to use as an offset on all future measurements

    //Clear the display
    oled.clear();
    
    //Write fixed text to diaplay
    oled.println(F("Angle"));
    oled.println(F("Max"));
    oled.println(F("Time"));
    oled.println(F("Max"));

    //Store internal timer value used to time updating of the display
    LastDispUD = millis();

    //Note that initialisation complete so on subsequent loops normal measurements take place
    Calibrated  = 1; 
  }
  else
  {
    //Main measurement loop
    //Calculate Calibrated angle by subtracting the offset measured initially
    Angle = Angle - AngleDatum; 

    //Add the latest measurement to the sum of measurements and increment the number of measuremenrts taken
    SumAngle += Angle; //+= means add to current value
    NMeasurements++;  //++ means add 1 to value

    //If the time over which averaging is calculated has elapsed... (checked by calling a function that compares the current time, start time and interval required)
    if (IntervalCheck(AverageDatum, AveragingTime))
    {
      //Calculate average angle by dividing sum of values by number of measurements taken
      AverageAngle = SumAngle / NMeasurements;

      //If average measured angle exceeds previous maximum, update the maximum value
      if(AverageAngle > MaxAngle)
      {
        MaxAngle = AverageAngle;
      }

      //If the minimum required angle to count as a wheelie is exceeded, but didn't on the previous loop...
      if ((AverageAngle >= MinAngle) && (!TimerStarted))
      {
        //Start wheelie timer
        TimerStarted = 1; //Set started flag
        StartTime = millis(); //Store internal timer value
        digitalWrite(PinLEDY, HIGH); //Turn Yellow LED on (indicating wheelie in progress)
      }

      //If a wheelie is in progress...
      if (TimerStarted)
      {
        //Only replace duration of previous wheelie attempt if duration of new attempt exceeds minimum time threshold
        if(IntervalCheck(StartTime, MinTime))
        {
          TotalTime = millis() - StartTime; //Calculate elapsed time
        }
        
        //Update maximum wheelie time if previous value is exceeded
        if (TotalTime > MaxTime)
        {
          MaxTime = TotalTime;
          digitalWrite(PinLEDG, HIGH); //Turn Green LED on (indicating new record breaking time)
        }
        else
        {
          digitalWrite(PinLEDG, LOW); //Turn Green LED on (not a new record breaking time)
        }
        
        //If latest average angle does not meet the minimum criterion for a wheelie, end the wheelie attempt
        if (AverageAngle < MinAngle)
        {
          TimerStarted = 0; //Note a wheelie no longer in progress
          digitalWrite(PinLEDY, LOW); //Turn Yellow LED off (indicating wheelie not in progress)
        }
      }
      
      //Update Display at specified update interval
      if (IntervalCheck(LastDispUD, DisplayUDInt))
      {
        oled.setCursor(66,0); //Sets cursor position.  First value is column pixel 0 to 127 (each character is 5 pixels wide plus 1 pixel space, x 2 as double font size = 12 pixels per character).  Second value is the text row 0 to 7, each row is 8 pixels high, double font size is 2 rows high so only set to even row numbers (0, 2, 4 or 6)
        if (AverageAngle <= -100.0)
        {
          oled.print(AverageAngle,0); //second parameter sets 0 decimal places, required if value < -100
        }
        else
        {
          oled.print(AverageAngle,1); //display to 1 decimal place otherwise
        }
        oled.clearToEOL(); //Clear from current position to end of current line, to delete old value completely
        
        oled.setCursor(66,2); //Move to second line of text (3rd row)
        oled.print(MaxAngle,1); //Display maximum angle to 1 decimal place (cannot be negative as must meet the mminimum angle threshold)
        oled.clearToEOL(); //Clear from current position to end of current line, to delete old value completely
    
        oled.setCursor(66,4); //Move to third line of text (5th row)
        if ((TotalTime/1000.0) < 1000.0) //If less than 1000 seconds
        {
          oled.print((TotalTime/1000.0),1); //Convert milliseconds to seconds and display with one decimal place
        }
        else
        {
          oled.print((TotalTime/1000.0),0); //Convert milliseconds to seconds and display with no decimal places
        }
        oled.clearToEOL(); //Clear from current position to end of current line, to delete old value completely

        oled.setCursor(66,6); //Move to fourth line of text (7th row)
        if ((MaxTime/1000.0) < 1000.0) //If less than 1000 seconds
        {
          oled.print((MaxTime/1000.0),1); //Convert milliseconds to seconds and display with one decimal place
        }
        else
        {
          oled.print((MaxTime/1000.0),0); //Convert milliseconds to seconds and display with no decimal places
        }
        oled.clearToEOL(); //Clear from current position to end of current line, to delete old value completely
  
        LastDispUD = millis(); //Store internal timer value when display last updated
      }
      
      //Reset averaging parameters for new averaging period
      AverageDatum = millis(); //Timer value at start of measurement period
      NMeasurements = 0; //No measurements made
      SumAngle=0.0; //Sum of measured values is zero
    }
  }
}

//__________

//Functions that can be called from elsewhere in the program

//Time interval check: compares current time to the start time (Datum) and required minimum Interval.  Returns 1 if Interval has been exceeded or 0 if it has not.
byte IntervalCheck(unsigned long Datum, unsigned long Interval) 
{
  unsigned long CurrentTime = millis(); //Read current internal timer value
  if ((CurrentTime - Datum) > Interval)
  {
   return 1; //Interval has been exceeded
  }
  else
  {
   return 0; //Interval has not been exceeded
  }
}