Hi,
I'm using the MPU-6050 gyro and an LED dot matrix (MAX2719). To visualise gyro readings, i've created a function that i've called in the loop which essentially should light up certain leds if the gyro roll output is below a certain value (using if else statement). Calling this in the loop severely slows down gyro readings. I've recorded a short clip here of the serial monitor where I rotate the gyro to 90deg without that function in the loop, and with it in the loop after. It doesn't even go to 90 the second time. Should the if else statement be this slow? Or have I implemented it wrong? I've attached my code, and the dot matrix function is the very last. The dot matrix does light up properly btw.
#include <Wire.h>
#include "LedControl.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;
LedControl lc=LedControl(12,10,11,3);
#define period 500
void setLED() {
/*
The MAX72XX is in power-saving mode on startup,
we have to do a wakeup call
*/
lc.shutdown(0,false);
/* Set the brightness to a medium values */
lc.setIntensity(0,1);
/* and clear the display */
lc.clearDisplay(0);
}
void setup() {
setLED();
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....
//Accelerometer angle calculations....
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);
writeArduinoOnMatrix();
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();
}
void writeArduinoOnMatrix() {
if (angle_pitch_output<=45){
byte a[8]={B11110000,B11110000,B11110000,B11110000,B11110000,B11110000,B11110000,B11110000};
/* now display them one by one with a small delay */
lc.setRow(0,0,a[0]);
lc.setRow(0,1,a[1]);
lc.setRow(0,2,a[2]);
lc.setRow(0,3,a[3]);
lc.setRow(0,4,a[4]);
lc.setRow(0,5,a[5]);
lc.setRow(0,6,a[6]);
lc.setRow(0,7,a[7]);
lc.setRow(0,8,a[8]);
}
else {
byte a[8]={B00001111,B00001111,B00001111,B00001111,B00001111,B00001111,B00001111,B00001111};
/* now display them one by one with a small delay */
lc.setRow(0,0,a[0]);
lc.setRow(0,1,a[1]);
lc.setRow(0,2,a[2]);
lc.setRow(0,3,a[3]);
lc.setRow(0,4,a[4]);
lc.setRow(0,5,a[5]);
lc.setRow(0,6,a[6]);
lc.setRow(0,7,a[7]);
lc.setRow(0,8,a[8]);
}
}