I just started learning c++ yesterday because I'm attempting to make a musical instrument controlled by a gyroscope and I am just trying to get some consistant values so that I have some semblance of control. Because I just started, I don't exactly know what is going on, but I found some basic gyroscope code that gives me raw values for both the gyro and the accelerometer and can be found here. 6_DOF_Sensor-MPU6050__SKU_SEN0142_-DFRobot I wanted to take the raw angular velocity values and integrate them so that it would give me the angular position and this was my attempt at trying to write a sketch for calibration then for finding the total area.
And when I use the original code the fluctuations are similar so I am wondering if there is some way to compensate for these fluctuations other than buying a new breakout board. I am new with c++ syntax so please tell me if my math or syntax is incorrect if that could be causing the problem. I just am not sure what to do now.
Heres my sketch:
In case it is unclear,
i=an arbitrary variable to end calibration
ax=accelerometer x value
prax=previous accel x value
dax=difference between prax and ax
avgax=average between ax and prax
calax=calibration offset
iax=integrated ax and prax via trapaziod rule
tax=total ax
follow the same format for the other axes and the gyroscope
/*
# Product: 6 DOF Sensor-MPU6050
# SKU : SEN0142
# Description:
# To read accel/gyro data from 6 DOF Sensor
*/
#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"
MPU6050 accelgyro;
int16_t ax, ay, az; // define accel as ax,ay,az
int16_t gx, gy, gz; // define gyro as gx,gy,gz
int i = -1;
int prax, dax, avgax, calax, iax;
int pray, day, avgay, calay, iay;
int praz, daz, avgaz, calaz, iaz;
int prgx, dgx, avggx, calgx, igx;
int prgy, dgy, avggy, calgy, igy;
int prgz, dgz, avggz, calgz, igz;
int ti = 10;
int tax = 0;
int tay = 0;
int taz = 0;
int tgx = 0;
int tgy = 0;
int tgz = 0;
void setup() {
Wire.begin(); // join I2C bus
Serial.begin(38400); // initialize serial communication
Serial.println("Initializing I2C devices...");
accelgyro.initialize();
// verify connection
Serial.println("Testing device connections...");
Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
for(i=-1; i<0; i--) {
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
ax = prax;
ay = pray;
az = praz;
gx = prgx;
gy = prgy;
gz = prgz;
delay(500);
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
dax = ax - prax;
abs(dax);
avgax = (ax + prax) / 2;
day = ay - pray;
abs(day);
avgay = (ay + pray) / 2;
daz = az - praz;
abs(daz);
avgaz = (az + praz) / 2;
dgx = gx - prgx;
abs(dgx);
avggx = (gx + prgx) / 2;
dgy = gy - prgy;
abs(dgy);
avggy = (gy + prgy) / 2;
dgz = gz - prgz;
abs(dgz);
avggz = (gz + prgz) / 2;
if (dax <= 2, day <= 2, daz <= 2, dgx <= 2, dgy <= 2, dgz <= 2){
i = 11;
calax = avgax;
calay = avgay;
calaz = avgaz;
calgx = avggx;
calgy = avggy;
calgz = avggz;
Serial.println("Calibrated");
}
else {
Serial.println("Hold Still");
}
}
}
void loop() {
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); // read measurements from device
ax = prax;
ay = pray;
az = praz;
gx = prgx;
gy = prgy;
gz = prgz;
delay(ti);
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
iax = ((prax + ax) / 2 - calax) * ti;
iay = ((pray + ay) / 2 - calay) * ti;
iaz = ((praz + az) / 2 - calaz) * ti;
igx = ((prgx + gx) / 2 - calgx) * ti;
igy = ((prgy + gy) / 2 - calgy) * ti;
igz = ((prgz + gz) / 2 - calgz) * ti;
tax += iax;
tay += iay;
taz += iaz;
tgx += igx;
tgy += igy;
tgz += igz;
// display tab-separated accel/gyro x/y/z values \t = tab
Serial.println("ax:\t");
Serial.println(tax);
Serial.println("ay:\t");
Serial.println(tay);
Serial.println("az:\t");
Serial.println(taz);
Serial.println("gx:\t");
Serial.println(tgx);
Serial.println("gy:\t");
Serial.println(tgy);
Serial.println("gz:");
Serial.println(tgz);
Serial.println(" ");
delay(50);
}
Here's some sample output while it is sitting still on the table:
I'll put it as code to save space.
Testing device connections...
MPU6050 connection successful
Calibrated
ax:
140
ay:
-100
az:
-860
gx:
-30
gy:
80
gz:
0
ax:
220
ay:
-920
az:
-620
gx:
-30
gy:
230
gz:
30
ax:
340
ay:
-1620
az:
-340
gx:
-180
gy:
330
gz:
-60
ax:
940
ay:
-1200
az:
-340
gx:
-130
gy:
530
gz:
20
ax:
980
ay:
-1520
az:
-200
gx:
-210
gy:
470
gz:
140
ax:
540
ay:
-1940
az:
-80
gx:
-180
gy:
470
gz:
80
ax:
440
ay:
-2520
az:
-180
gx:
-250
gy:
510
gz:
200
ax:
700
ay:
-2180
az:
-360
gx:
-260
gy:
520
gz:
270
ax:
340
ay:
-2720
az:
-420
gx:
-220
gy:
470
gz:
220