I need to operate 2 different sensors (MPU 6050 and Admp 401) simultaneously. I have managed to run each one of them separately and I don't know how to manipulate the code so they could work together. My background in coding is basic and I'm not the one who wrote these codes.
Here are the sketches:
1.Code used for IMU (MPU-6050)
// CONNECTIONS
//Gyro - Arduino UNO R3
//VCC - 5V
//GND - GND
//SDA - A4
//SCL - A5
//INT - port-2
#include <Wire.h>
//Declaring some global variables
int gyro_x, gyro_y, gyro_z;
long gyro_x_cal, gyro_y_cal, gyro_z_cal;
boolean set_gyro_angles;
long acc_x, acc_y, acc_z, acc_total_vector;
float angle_roll_acc, angle_pitch_acc;
float angle_pitch, angle_roll;
int angle_pitch_buffer, angle_roll_buffer;
float angle_pitch_output, angle_roll_output;
long loop_timer;
int temp;
void setup() {
Wire.begin(); //Start I2C as master
setup_mpu_6050_registers(); //Setup the registers of the MPU-6050
for (int cal_int = 0; cal_int < 1000 ; cal_int ++){ //Read the raw acc and gyro data from the MPU-6050 for 1000 times
read_mpu_6050_data();
gyro_x_cal += gyro_x; //Add the gyro x offset to the gyro_x_cal variable
gyro_y_cal += gyro_y; //Add the gyro y offset to the gyro_y_cal variable
gyro_z_cal += gyro_z; //Add the gyro z offset to the gyro_z_cal variable
delay(3); //Delay 3us to have 250Hz for-loop
}
// divide by 1000 to get avarage offset
gyro_x_cal /= 1000;
gyro_y_cal /= 1000;
gyro_z_cal /= 1000;
Serial.begin(115200);
loop_timer = micros(); //Reset the loop timer
}
void loop(){
read_mpu_6050_data();
//Subtract the offset values from the raw gyro values
gyro_x -= gyro_x_cal;
gyro_y -= gyro_y_cal;
gyro_z -= gyro_z_cal;
//Gyro angle calculations . Note 0.0000611 = 1 / (250Hz x 65.5)
angle_pitch += gyro_x * 0.0000611; //Calculate the traveled pitch angle and add this to the angle_pitch variable
angle_roll += gyro_y * 0.0000611; //Calculate the traveled roll angle and add this to the angle_roll variable
//0.000001066 = 0.0000611 * (3.142(PI) / 180degr) The Arduino sin function is in radians
angle_pitch += angle_roll * sin(gyro_z * 0.000001066); //If the IMU has yawed transfer the roll angle to the pitch angel
angle_roll -= angle_pitch * sin(gyro_z * 0.000001066); //If the IMU has yawed transfer the pitch angle to the roll angel
//Accelerometer angle calculations
acc_total_vector = sqrt((acc_xacc_x)+(acc_yacc_y)+(acc_zacc_z)); //Calculate the total accelerometer vector
//57.296 = 1 / (3.142 / 180) The Arduino asin function is in radians
angle_pitch_acc = asin((float)acc_y/acc_total_vector) 57.296; //Calculate the pitch angle
angle_roll_acc = asin((float)acc_x/acc_total_vector)* -57.296; //Calculate the roll angle
angle_pitch_acc -= 0.0; //Accelerometer calibration value for pitch
angle_roll_acc -= 0.0; //Accelerometer calibration value for roll
if(set_gyro_angles){ //If the IMU is already started
angle_pitch = angle_pitch * 0.9996 + angle_pitch_acc * 0.0004; //Correct the drift of the gyro pitch angle with the accelerometer pitch angle
angle_roll = angle_roll * 0.9996 + angle_roll_acc * 0.0004; //Correct the drift of the gyro roll angle with the accelerometer roll angle
}
else{ //At first start
angle_pitch = angle_pitch_acc; //Set the gyro pitch angle equal to the accelerometer pitch angle
angle_roll = angle_roll_acc; //Set the gyro roll angle equal to the accelerometer roll angle
set_gyro_angles = true; //Set the IMU started flag
}
//To dampen the pitch and roll angles a complementary filter is used
angle_pitch_output = angle_pitch_output * 0.9 + angle_pitch * 0.1; //Take 90% of the output pitch value and add 10% of the raw pitch value
angle_roll_output = angle_roll_output * 0.9 + angle_roll * 0.1; //Take 90% of the output roll value and add 10% of the raw roll value
Serial.print(" | Angle = "); Serial.println(angle_pitch_output);
while(micros() - loop_timer < 4000); //Wait until the loop_timer reaches 4000us (250Hz) before starting the next loop
loop_timer = micros();//Reset the loop timer
}
void setup_mpu_6050_registers(){
//Activate the MPU-6050
Wire.beginTransmission(0x68); //Start communicating with the MPU-6050
Wire.write(0x6B); //Send the requested starting register
Wire.write(0x00); //Set the requested starting register
Wire.endTransmission();
//Configure the accelerometer (+/-8g)
Wire.beginTransmission(0x68); //Start communicating with the MPU-6050
Wire.write(0x1C); //Send the requested starting register
Wire.write(0x10); //Set the requested starting register
Wire.endTransmission();
//Configure the gyro (500dps full scale)
Wire.beginTransmission(0x68); //Start communicating with the MPU-6050
Wire.write(0x1B); //Send the requested starting register
Wire.write(0x08); //Set the requested starting register
Wire.endTransmission();
}
void read_mpu_6050_data(){ //Subroutine for reading the raw gyro and accelerometer data
Wire.beginTransmission(0x68); //Start communicating with the MPU-6050
Wire.write(0x3B); //Send the requested starting register
Wire.endTransmission(); //End the transmission
Wire.requestFrom(0x68,14); //Request 14 bytes from the MPU-6050
while(Wire.available() < 14); //Wait until all the bytes are received
acc_x = Wire.read()<<8|Wire.read();
acc_y = Wire.read()<<8|Wire.read();
acc_z = Wire.read()<<8|Wire.read();
temp = Wire.read()<<8|Wire.read();
gyro_x = Wire.read()<<8|Wire.read();
gyro_y = Wire.read()<<8|Wire.read();
gyro_z = Wire.read()<<8|Wire.read();
}
2.Code used for MEMS mic (ADMP401)
int mic = A0; // A0 is the audio output of the pin, and since sound is analog, its connected to the analog pin in arduino
const int sampleTime = 2; // sampling time for the input audio sound is 2ms
int micOut;
void setup() {
Serial.begin(115200);
}
void loop() {
int micOutput = findPTPAmp(); // we use a PTPAmp function to find the peak to peak amplitude
VUMeter(micOutput);
}
int findPTPAmp(){
unsigned long startTime= millis(); // Start of sample window
unsigned int PTPAmp = 0;
// the above two are 2 time variables to find PTP amplitude
unsigned int maxAmp = 0;
unsigned int minAmp = 1023;
// these 2 are the signal variables to find PTP AMplitude
// We basically try to Find the max and min of the mic output within the 2 ms timeframe
while(millis() - startTime < sampleTime)
{
micOut = analogRead(mic);
if( micOut < 1023) //to prevent erroneous readings; cause output can’t be greater than 1023
{
if (micOut > maxAmp)
{
maxAmp = micOut; //save only the max reading
}
else if (micOut < minAmp)
{
minAmp = micOut; //save only the min reading
}
}
}
PTPAmp = maxAmp - minAmp;
double micOut_Volts = (PTPAmp * 3.3) / 1023; // Convert ADC into voltage formula ; 3.3 is the Vcc.
//Uncomment this line for help debugging (be sure to also comment out the VUMeter function)
//Serial.println(PTPAmp);
return PTPAmp;
}
// Volume Unit Meter function: map the PTP amplitude to a volume unit between 0 and 10.
// Map the mic PTP Amp to a volume unit between 0 and 10.
// We use Amplitude instead of voltage to give a larger and more accurate range for the map function that we use next in the code.
int VUMeter(int micAmp){
int preValue = 0;
int fill = map(micAmp, 23, 750, 0, 10);
// val = map(val, 0, 1023, 0, 255);
val equals value of analogRead(0)
0, 1023 is the range of values that a reading from analogRead(0) could be.
0,255 is the range of values you are converting the value to.
// only print the volume unit value if it differs from the previous value
while(fill != preValue)
{
Serial.println(fill);
preValue = fill;
}
}
