Version 1

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);
+}
\ No newline at end of file