Store an MPU6050's value in array?!

Projects Programming
1

Hello.

I would like to store the MPU's angle value in an array, but I dont know how to start it.

I have a code where I calculate the angle around Y axis, and from this values in a given time I'd like to calculate an average.

Here is my code, where I calculate the angle and send the values to another ESP. I would like to send the average angle.

#include <Arduino.h>
#include <ESP8266WiFi.h>
#include <Wire.h>
#include <espnow.h>

int16_t Acc_rawX, Acc_rawY, Acc_rawZ,Gyr_rawX, Gyr_rawY, Gyr_rawZ;

const int MPU = 0x68;
float Acc_angle_X, Acc_angle_Y;
float Gyro_angle_X, Gyro_angle_Y;
float Angle_X, Angle_Y;
float elapsedTime, t, timePrev;
float rad_to_deg = 180/3.141592654;

//reciever MAC address
uint8_t broadcastAddress[] = {0x98, 0xF4, 0xAB, 0x6C, 0x57, 0xC4};

void OnDataSent(uint8_t *mac_addr, uint8_t sendStatus) {
  Serial.print("Last Packet Send Status: ");
  if (sendStatus == 0){
    Serial.println("Delivery success");
  }
  else{
    Serial.println("Delivery fail");
  }
}

void setup() {
  Serial.begin(115200);
  Wire.begin(); 
  Wire.beginTransmission(MPU);
  Wire.write(0x6B);
  Wire.write(0);
  Wire.endTransmission(true);
  
  WiFi.mode(WIFI_STA);
  // Init ESP-NOW
  if (esp_now_init() != 0) {
    Serial.println("Error initializing ESP-NOW");
    return;
  }

  // Once ESPNow is successfully Init, we will register for Send CB to
  // get the status of Trasnmitted packet
  esp_now_set_self_role(ESP_NOW_ROLE_CONTROLLER);
  esp_now_register_send_cb(OnDataSent);
  
  // Register peer
  esp_now_add_peer(broadcastAddress, ESP_NOW_ROLE_SLAVE, 1, NULL, 0);

   t = millis();
}
 
void loop(){ 
  // Accelerometer data //
  Wire.beginTransmission(MPU);
  Wire.write(0x3B); //Ask for the 0x3B register- correspond to AcX
  Wire.endTransmission(false);
  Wire.requestFrom(MPU,6,true);

  Acc_rawX=Wire.read()<<8|Wire.read(); //each value needs two registres
  Acc_rawY=Wire.read()<<8|Wire.read();
  Acc_rawZ=Wire.read()<<8|Wire.read();
  /*---X---*/
  Acc_angle_X = atan((Acc_rawY/16384.0)/sqrt(pow((Acc_rawX/16384.0),2) + pow((Acc_rawZ/16384.0),2)))*rad_to_deg;
  /*---Y---*/
  Acc_angle_Y = atan(-1*(Acc_rawX/16384.0)/sqrt(pow((Acc_rawY/16384.0),2) + pow((Acc_rawZ/16384.0),2)))*rad_to_deg;

  // Gyroscope data //
  timePrev = t;
  t = millis();
  elapsedTime = (t - timePrev) / 1000;
  
  Wire.beginTransmission(MPU);
  Wire.write(0x43); //Gyro data first adress
  Wire.endTransmission(false);
  Wire.requestFrom(MPU,4,true); //Just 4 registers
   
  Gyr_rawX=Wire.read()<<8|Wire.read(); 
  Gyr_rawY=Wire.read()<<8|Wire.read();

  /*---X axis angle---*/
  Angle_X = 0.98 *(Angle_X + Gyro_angle_X*elapsedTime) + 0.02*Acc_angle_X;
  /*---Y axis angle---*/
  Angle_Y = 0.98 *(Angle_Y + Gyro_angle_Y*elapsedTime) + 0.02*Acc_angle_Y;

  Serial.print("Angle: ");
  Serial.println(Angle_Y);
  
  esp_now_send(broadcastAddress, (uint8_t *) &Angle_Y, sizeof(Angle_Y));
  
}

My idea was that, write the obtained values into an array, then averaged the array's element, and send it. But unfortunately I've never worked with arrays.

Thanks in advance!

2

I believe what you want to do is the same as what is commonly done with analogs called "smoothing". Every time you get a new angle then calculate a new running average. Here is an example with analogs so jut adapt for your angles:

Smoothing

3

I use a simple Kalman filter to smooth data.

Works good lasts a long time.

4

ToddL1962:
I believe what you want to do is the same as what is commonly done with analogs called "smoothing". Every time you get a new angle then calculate a new running average. Here is an example with analogs so jut adapt for your angles:

Smoothing

Thanks for answering!

Well, not exactly.
My gyroscope give the angle values continiously. And what I would like to do is to collect these angle values in for example 200elements long array, and when it is full then calculate average and send to the another ESP.

I've never heard about smoothing or Kalman filter, it can be happen these are the same what I want to do.