Hello!
This is the first time I've asked a forum for help. I'm pretty new at programming these incredible devices, but they are fun. I am currently trying to run two .91" OLED displays and an accelerometer off of an Arduino Nano using a TCA9548A multiplexer. My code compiles and uploads, but I'm having issues. I get the displays to initialize, but none of my data displays. The main thing I'm writing for though is this: It seems the void loop() function isn't looping. It runs through the entire code and then just stops/hangs. I have used the serial monitor a lot for trying to troubleshoot, so every "Serial.print" line you see in my code below is only for troubleshooting. Those lines do not do anything for the end result.
Any assistance will be greatly appreciated. Thank you for your time.
Here is my code:
/*
Electronic Level
Uses MPU-6050 IMU
Displays on 2 ea. .91" OLED displays and row of LED bulbs
*/
#include <Arduino.h>
#include <SPI.h>
// Include Wire Library for I2C
#include <Wire.h>
extern "C" {
#include "utility/twi.h" // from Wire Library, so we can do bus scanning
}
// Include two libraries for displays
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#define TCAADDR 0x70
#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 32 // OLED display height, in pixels
#define OLED_RESET -1 // Reset Pin # (or -1 if sharing Arduino reset pin)
#define PIN_SDA A4
#define PIN_SCL A5
#define PIN_INT RST
/* Declaration for an SSD1306 display connected to I2C (SDA, SCL pins) */
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);
// Reset pin not used but needed for library
#define OLED_RESET 10
//Variables for Gyroscope
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;
// Setup timers and temp variables
long loop_timer;
int temp;
// Display counter
int displaycount = 0;
int myCounter = 0;
void tcaselect(uint8_t i) {
if (i > 7) return;
Wire.beginTransmission(TCAADDR);
Wire.write(1 << i);
Wire.endTransmission();
}
void setup() {
while (!Serial);
delay(1000);
//Start I2C
Wire.begin();
Serial.begin(9600);
display.begin();
/* Set multiplexer to channel 2 and initialize OLED-0 with I2C addr 0x3C */
tcaselect(2);
if (!display.begin(0x3C)) {
Serial.println(F("SSD1306 allocation failed"));
for (;;); // Don't proceed, loop forever
}
display.display();
delay(2000);
display.clearDisplay();
// Print on first row of LCD
display.setCursor(0, 0);
display.setTextSize(1);
display.write("HELLO!");
display.display();
/* Initialize OLED-1 to channel 4 with I2C addr 0x3C */
tcaselect(4);
if (!display.begin(0x3C)) {
Serial.println(F("SSD1306 allocation failed"));
for (;;); // Don't proceed, loop forever
}
display.display();
delay(2000);
display.clearDisplay();
// Print on first row of LCD
display.setCursor(0, 0);
display.setTextSize(1);
display.write("HELLO!");
display.display();
//Setup the registers of the MPU-6050
tcaselect(7);
setup_mpu_6050_registers();
//Read the raw acc and gyro data from the MPU-6050 1000 times
for (int cal_int = 0; cal_int < 1000 ; cal_int ++) {
read_mpu_6050_data();
//Add the gyro x offset to the gyro_x_cal variable
gyro_x_cal += gyro_x;
//Add the gyro y offset to the gyro_y_cal variable
gyro_y_cal += gyro_y;
//Add the gyro z offset to the gyro_z_cal variable
gyro_z_cal += gyro_z;
//Delay 3us to have 250Hz for-loop
delay(3);
}
// Divide all results by 1000 to get average offset
gyro_x_cal /= 1000;
gyro_y_cal /= 1000;
gyro_z_cal /= 1000;
// Init Timer
//loop_timer = micros();
// Troubleshoot 1
Serial.println("Through Set-up section");
Serial.print("x = ");
Serial.print(gyro_x_cal);
Serial.print(" : y = ");
Serial.print(gyro_y_cal);
Serial.print(" : z = ");
Serial.println(gyro_z_cal);
Serial.println(" ");
}
void loop() {
// Get data from MPU-6050
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;
//Calculations for accelerometer
my_calculations();
//Counting the loops - only display every n-th reading
my_counters();
}
void my_calculations() {
// Troubleshoot 2
Serial.println("Start of Loop section");
myCounter = myCounter + 1;
Serial.print("x = ");
Serial.print(gyro_x);
Serial.print(" : y = ");
Serial.print(gyro_y);
Serial.print(" : z = ");
Serial.print(gyro_z);
Serial.println("....");
Serial.print("Counter = ");
Serial.println(myCounter);
//Gyro angle calculations . Note 0.0000611 = 1 / (250Hz x 65.5)
//Calculate the traveled pitch angle and add this to the angle_pitch variable
angle_pitch += gyro_x * 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_roll += gyro_y * 0.0000611;
//If the IMU has yawed transfer the roll angle to the pitch angle
angle_pitch += angle_roll * sin(gyro_z * 0.000001066);
//If the IMU has yawed transfer the pitch angle to the roll angle
angle_roll -= angle_pitch * sin(gyro_z * 0.000001066);
//Accelerometer angle calculations
//Calculate the total accelerometer vector
acc_total_vector = sqrt((acc_x * acc_x) + (acc_y * acc_y) + (acc_z * acc_z));
//57.296 = 1 / (3.142 / 180) The Arduino asin function is in radians
//Calculate the pitch angle
angle_pitch_acc = asin((float)acc_y / acc_total_vector) * 57.296;
//Calculate the roll angle
angle_roll_acc = asin((float)acc_x / acc_total_vector) * -57.296;
//Accelerometer calibration value for pitch
angle_pitch_acc -= 0.0;
//Accelerometer calibration value for roll
angle_roll_acc -= 0.0;
if (set_gyro_angles) {
//If the IMU has been running
//Correct the drift of the gyro pitch angle with the accelerometer pitch angle
angle_pitch = angle_pitch * 0.9996 + angle_pitch_acc * 0.0004;
//Correct the drift of the gyro roll angle with the accelerometer roll angle
angle_roll = angle_roll * 0.9996 + angle_roll_acc * 0.0004;
}
else {
//IMU has just started
//Set the gyro pitch angle equal to the accelerometer pitch angle
angle_pitch = angle_pitch_acc;
//Set the gyro roll angle equal to the accelerometer roll angle
angle_roll = angle_roll_acc;
//Set the IMU started flag
set_gyro_angles = true;
}
//To dampen the pitch and roll angles a complementary filter is used
//Take 90% of the output pitch value and add 10% of the raw pitch value
angle_pitch_output = angle_pitch_output * 0.9 + angle_pitch * 0.1;
//Take 90% of the output roll value and add 10% of the raw roll value
angle_roll_output = angle_roll_output * 0.9 + angle_roll * 0.1;
//Wait until the loop_timer reaches 4000us (250Hz) before starting the next loop
// Print to Serial Monitor
//Serial.print(" | Angle = "); Serial.println(angle_pitch_output);
return;
}
void setup_mpu_6050_registers() {
//Activate the MPU-6050
//Start communicating with the MPU-6050
Wire.beginTransmission(0x68);
//Send the requested starting register
Wire.write(0x6B);
//Set the requested starting register
Wire.write(0x00);
//End the transmission
Wire.endTransmission();
//Configure the accelerometer (+/-8g)
//Start communicating with the MPU-6050
Wire.beginTransmission(0x68);
//Send the requested starting register
Wire.write(0x1C);
//Set the requested starting register
Wire.write(0x10);
//End the transmission
Wire.endTransmission();
//Configure the gyro (500dps full scale)
//Start communicating with the MPU-6050
Wire.beginTransmission(0x68);
//Send the requested starting register
Wire.write(0x1B);
//Set the requested starting register
Wire.write(0x08);
//End the transmission
Wire.endTransmission();
}
void my_counters() {
if (displaycount == 100) {
Serial.println("Troubleshoot 5 - in the IF statement");
screenPrint();
displaycount = 0;
} else {
// Increment the display counter
displaycount = displaycount + 1;
}
Serial.print("displaycount is: ");
Serial.println(displaycount);
return;
}
void screenPrint() {
tcaselect(2);
// Print on first row of LCD
display.display();
delay(1000);
display.clearDisplay();
display.setCursor(0, 0);
display.write("Pitch: ");
display.write(angle_pitch_output);
display.display();
Serial.println("Troubleshoot 3");
tcaselect(4);
display.display();
delay(1000);
display.clearDisplay();
display.setCursor(0, 0);
display.write("Roll: ");
display.write(angle_roll_output);
display.display();
Serial.println("Troubleshoot 4");
return;
}
void read_mpu_6050_data() {
//Read the raw gyro and accelerometer data
//Start communicating with the MPU-6050
Wire.beginTransmission(0x68);
//Send the requested starting register
Wire.write(0x3B);
//End the transmission
Wire.endTransmission();
//Request 14 bytes from the MPU-6050
Wire.requestFrom(0x68, 14);
//Wait until all the bytes are received
while (Wire.available() < 14);
//Following statements left shift 8 bits, then bitwise OR.
//Turns two 8-bit values into one 16-bit value
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();
}