Accelometer_test.cpp

#include <Arduino.h>
#include<Wire.h>
#include <math.h>



const int MPU_addr=0x68;  // I2C address of the MPU-6050


int16_t AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ;
double AccelerationX,AccelerationY,AccelerationZ;
double R,AngX,AngY,AngZ, NoGAccelX, NoGAccelY, NoGAccelZ;         //    Not sure if to use doubles or int_t or doubles_t or floats
float velocityX =0;
float velocityY =0;
float velocityZ =0;


bool first =true;

unsigned long last_time_sampled =0;     //  Used for finding velovity
unsigned long last_time_printed =0;     //  Used to to print




# define convertAdcMss   ( 9.80665 / (pow(2,14)))     //  Gravity / 2^14 . 
                                                      // This is because the range of the MPU is +-2G hence 2^16 represents 4 G.

#define DEBOUNCE_TIME 1000                            //  This is to debounce the buttons to problems with button.

volatile uint32_t DebounceTimerUp = 0;                //  Used to calcuale the current time since the button was allowed.
volatile uint32_t DebounceTimerDown = 0;              //  Used to calcuale the current time since the button was allowed.


#define PIN_BUTTON 4  // Pin to which the button, PIR motion detector or radar is connected


bool buttonpressdown = false;   //  Used to flag if the button is pressed.
bool buttonpressup = false;     // Used to flag is button is up aka foot is in the air.


//    #define PIN_LED 2      // On board LED used for debugging
//    #define DELAY_LED 2000



// These functions are not used yet. 
// The function is placed in the RAM of the ESP32. 
void IRAM_ATTR buttonpresseddown() {
  if ( millis() - DEBOUNCE_TIME  >= DebounceTimerDown ) {
    DebounceTimerDown = millis();                              
    buttonpressdown=true;
  } 
}
void IRAM_ATTR buttonpressedup() {
  if ( millis() - DEBOUNCE_TIME  >= DebounceTimerUp ) {
    DebounceTimerUp = millis();
    buttonpressup=true;
  } 
}

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);
  Serial.begin(9600);

  pinMode(PIN_BUTTON, INPUT_PULLDOWN);
  attachInterrupt(PIN_BUTTON, buttonpresseddown, RISING); // This is creating an interupt when the button is pressed.
  
}
void loop(){
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x3B);  // 
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_addr,14,true);  // 
  AcX=Wire.read()<<8|Wire.read();  //   Reads the accelormeter
  AcY=Wire.read()<<8|Wire.read();  // 
  AcZ=Wire.read()<<8|Wire.read();  // 
  // Tmp=Wire.read()<<8|Wire.read();  // Reads temp
  // GyX=Wire.read()<<8|Wire.read();  // Reads the gyroscope
  // GyY=Wire.read()<<8|Wire.read();  // 
  // GyZ=Wire.read()<<8|Wire.read();  // 
  
  AccelerationX=AcX*convertAdcMss;    //  Converts the input voltage into m/ss
  AccelerationY=AcY*convertAdcMss;
  AccelerationZ=AcZ*convertAdcMss;
  R = sqrt(pow(AccelerationX,2)+pow(AccelerationY,2)+pow(AccelerationZ,2));   //  Resultant vector of acceleration.



  NoGAccelX=AccelerationX - 9.80665*(AccelerationX/R);      //  Removes the acceleration due to graveity component of the acceleration.
  NoGAccelY=AccelerationY - 9.80665*(AccelerationY/R);
  NoGAccelZ=AccelerationZ - 9.80665*(AccelerationZ/R);

  // AngX= acos(AccelerationX/R) *180/PI;
  // AngY= acos(AccelerationY/R)*180/PI;        //    Calculates the angle of each axis
  //AngZ= acos(AccelerationZ/R)*180/PI;



  unsigned long now = micros();                                   //  This is the calcualtion of velocity, acceleration * time at each stage.
  if(abs(NoGAccelX)>0.5)                                          //  Time at each stage changed in calculated.
  velocityX += NoGAccelX * (now - last_time_sampled)/1000000;
  if(abs(NoGAccelY)>0.5)
  velocityY += NoGAccelY * (now - last_time_sampled)/1000000;
  if(abs(NoGAccelZ)>0.5)
  velocityZ += NoGAccelZ * (now - last_time_sampled)/1000000;
  last_time_sampled = now;

 if (now - last_time_printed >= 200000) {
        Serial.print("x = ");
        Serial.print(NoGAccelX);              //    Just changed to print every 0.2 seconds.
        Serial.print(", x = ");
        Serial.print(velocityX );
        Serial.print(" y = ");
        Serial.print(NoGAccelY);
        Serial.print(", y = ");
        Serial.print(velocityY );
        Serial.print(" z = ");
        Serial.print(NoGAccelZ);
        Serial.print(", z = ");
        Serial.println(velocityZ );
        last_time_printed = now;
    }

  delay(30);
}