Pedometer_Final

#include<Wire.h>
#include <LiquidCrystal_PCF8574.h>
LiquidCrystal_PCF8574 lcd(0x27);  // set the LCD address to 0x27 for a 16 chars and 2 line display
const int MPU_addr=0x68 , arraySize = 4;  // I2C address of the MPU-6050
int16_t AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ,count = 0;
int averagex[arraySize] , averagey[arraySize] , averagez[arraySize];
int avgZ ,steps = 0, higherthan = 0;
int threshold = 2300;
double time_1,time_2;
float distance ;
double Cal = 0;
int cnt = 0;
int arr[300];
void setup(){
  Wire.begin();
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x6B);  // PWR_MGMT_1 register
  Wire.write(0);     // set to zero (wakes up the MPU-6050)
  Wire.endTransmission(true);
  Wire.begin();
  Wire.beginTransmission(0x27);             //LCD
  Wire.endTransmission();
  Serial.begin(9600);
  lcd.begin(16, 2); // initialize the lcd

}
void loop(){
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_addr,14,true);  // request a total of 14 registers
  AcX=Wire.read()<<8|Wire.read();  // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
  AcY=Wire.read()<<8|Wire.read();  // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
  AcZ=Wire.read()<<8|Wire.read();  // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
  Tmp=Wire.read()<<8|Wire.read();  // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
  GyX=Wire.read()<<8|Wire.read();  // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
  GyY=Wire.read()<<8|Wire.read();  // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
  GyZ=Wire.read()<<8|Wire.read();  // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
  averagex[count % arraySize] = AcX;
  averagey[count % arraySize] = AcY;
  averagez[count % arraySize] = AcZ;
  count++;
  if(count >= 4){
    //Serial.print((averagex[0]+averagex[1]+averagex[2]+averagex[3]) / arraySize); Serial.print(" ");
    //Serial.print((averagey[0]+averagey[1]+averagey[2]+averagey[3]) / arraySize); Serial.print(" ");
    avgZ = (averagez[0]+averagez[1]+averagez[2]+averagez[3]) / arraySize ;
    //Serial.print(avgZ); Serial.print(" ");
    //Serial.print("\n");
  }
  if (avgZ > threshold && higherthan == 0){
    higherthan = 1 ;
  } else if (avgZ < threshold && higherthan == 1){
  if(check_time_condition() == 1)
  {
    double startingTime;
    if(steps == 0)
    {
      startingTime = millis()/1000 - 1;
      Cal = 0;
    }
    steps++;
    distance = steps * 0.45;
    print_first_line(steps,distance);
    if(steps < 2)
    {
      print_second_line(0,0);
    }else
    {
    double velocity = distance / (millis()/1000 - startingTime);
    Cal = Cal +  velocity * 4.5;
    print_second_line(velocity,Cal);
    }
  }
    higherthan = 0;
  }

  /*Serial.print("AcX = ");*/ //Serial.print(AcX); Serial.print(" ");
  /*Serial.print(" | AcY = ");*/ //Serial.print(AcY); Serial.print(" ");
  /*Serial.print(" | AcZ = ");*/ //Serial.print(AcZ); Serial.print(" ");
  //Serial.print("\n");
  delay(50);
}

int check_time_condition()
{
  if(cnt %2 == 0)
  {
    time_1 = millis() / 100;
  }else if(cnt % 2 == 1)
  {
    time_2 = millis() / 100;
  }
     cnt++;
   if(cnt == 1)
     return 1 ;
   else
   {
    double diff =time_1 - time_2;
     if((time_1 - time_2 > 0 && time_1 - time_2 < 4) || (time_2 - time_1 > 0 &&time_2 - time_1 < 4) )
     {
       return 0;           //don't count
     }else if( time_1 - time_2 > 20 ||time_2 - time_1 > 20)
     {
       return 0;           //count normally
     }else
       return 1;
   }

}

void print_first_line(int steps , double distance)
{
  lcd.setBacklight(255);
     lcd.home();
     lcd.clear();
     lcd.setCursor(0, 0);
     lcd.print("steps:");
     lcd.setCursor(6, 0);
     lcd.print(steps);
     lcd.setCursor(8, 0);
     lcd.print("dis:");
     lcd.setCursor(12, 0);
     lcd.print(distance);
}

void print_second_line(double velocity,double Cal)
{
  lcd.setCursor(0, 1);
     lcd.print("vel:");
     lcd.setCursor(4, 1);
     lcd.print(velocity);
     lcd.setCursor(8, 1);
     lcd.print("Cal:");
     lcd.setCursor(12, 1);
     lcd.print(Cal);
}