Hi, I'm trying to run a system here with a rotary encoder, an MPU6050, and an SSD1306 (Circuit diagram below.) The MPU and OLED screen were not working together and after a little bit of research I found that they both need the I2C pins so I got a TCA9548A multiplexer. From all that I read, I should have hooked it up right but neither the MPU or the screen register.
Both the MPU and screen turn on independently. The MPU light is on, and from the debug statements, the prints say the screen is connected but the screen does not turn on. What am I doing wrong?
Thanks
Circuit Diagram
Code
// Load Libraries
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>
#include <SPI.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <Button.h>
// Create class
#define OLED_RESET -1
#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 32 // OLED display height, in pixels
Adafruit_MPU6050 mpu;
Button calib1(5);
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);
// Rotary Encoder Inputs
#define CLK 3
#define DT 4
int counter = 0, currentStateCLK, lastStateCLK, clickNum = 360;
const int trigger = 2, calTrigger = trigger * 3;
String currDir ="", prevDir ="Idle", mode;
int mainWheelRadiusIn = 24; // In
float mainWheelRadiusM = mainWheelRadiusIn * 0.254; // Meters
float angle;
unsigned long currMillis, startTime, finalTime, currDur, btDur, totDur;
const long pauseDuration = 1000; // Miliseconds
const long duration = 2000; // Miliseconds
float angAcceli, angAccelm, angAccelf, angVelf, avgAngAccel;
float maxUserSpeed, currUserSpeed, speedRatio;
int maxMotorSpeed = 1.7, currMotorSpeed;
// Vars for Sensor Data
sensors_event_t a, g, temp;
const int numSamples = 1000; // Number of samples to collect for calibration
const int calibrateDelay = 10; // Delay in milliseconds between samples
float axOffset = 0, ayOffset = 0, azOffset = 0; // Calibration offsets for accelerometer
float axCalibrated = 0, ayCalibrated = 0, azCalibrated = 0;
float tiltX = 0, tiltY = 0, tiltZ = 0;
void calibrateMPU(){
float axSum = 0, aySum = 0, azSum = 0;
//robojaxText("Calibrating MPU6050. Please keep the sensor still...", 4, 3, 1, false);
Serial.println(F("Calibrating MPU6050. Please keep the sensor still..."));
delay(calibrateDelay); // Initial delay before starting calibration
for (int i = 0; i < numSamples; i++) {
// Get new sensor events with the readings
mpu.getEvent(&a, &g, &temp);
axSum += a.acceleration.x;
aySum += a.acceleration.y;
azSum += a.acceleration.z;
delay(calibrateDelay);
}
axOffset = axSum / numSamples;
ayOffset = aySum / numSamples;
azOffset = azSum / numSamples;
Serial.println(F("MPU Calibration complete!"));
Serial.print(F("Accelerometer offsets (X, Y, Z): "));
Serial.print((axOffset));
Serial.print(F(", "));
Serial.print((ayOffset));
Serial.print(F(", "));
Serial.println((azOffset));
}
void getMPUData(){
mpu.getEvent(&a, &g, &temp);
axCalibrated = a.acceleration.x - axOffset;
ayCalibrated = a.acceleration.y - ayOffset;
azCalibrated = a.acceleration.z - azOffset;
tiltX = atan2(ayCalibrated, azCalibrated) * (180 / PI);
tiltY = atan2(axCalibrated, azCalibrated) * (180 / PI);
tiltZ = atan2(ayCalibrated, axCalibrated) * (180 / PI);
// Print the calculated values
Serial.print(F("X-Axis Acceleration (m/s^2): "));
Serial.println((axCalibrated));
Serial.print(F("Tilt Angle of X-Axis (degrees): "));
Serial.println((tiltX));
Serial.print(F("Y-Axis Acceleration (m/s^2): "));
Serial.println((ayCalibrated));
Serial.print(F("Tilt Angle of Y-Axis (degrees): "));
Serial.println((tiltY));
Serial.print(F("Z-Axis Acceleration (m/s^2): "));
Serial.println((azCalibrated));
Serial.print(F("Tilt Angle of Z-Axis (degrees): "));
Serial.println((tiltZ));
}
void resetCounter(){
if ((abs(counter) >= trigger) || (currDir != prevDir) || (btDur >= pauseDuration)){
counter = 0;
}
return;
}
float getAngAccel(){
getMPUData();
// Calculation
float angAccel = axCalibrated / mainWheelRadiusM;
return angAccel;
}
float getAngVel( float angAccel, float finalAng){
float angVel = sqrt(2 * angAccel * (finalAng));
return angVel;
}
float getLinVel(float angVel, float mainWheelRadiusM){
float linVel = angVel * mainWheelRadiusM;
return linVel;
}
/*
* robojaxText(String text, int x, int y,int size, boolean d)
* text is the text string to be printed
* x is the integer x position of text
* y is the integer y position of text
* z is the text size, 1, 2, 3 etc
* d is either "true" or "false". Not sure, use true
*/
void robojaxText(String text, int x, int y, int size, boolean d) {
display.setTextSize(size);
display.setTextColor(WHITE);
display.setCursor(x, y);
display.println(text);
if (d) {
display.display();
}
}
void calibrateEncoders(){
// Serial.println(F(mode));
readTime();
// Read the current state of CLK
currentStateCLK = digitalRead(CLK);
if (currentStateCLK != lastStateCLK && currentStateCLK == 1){
// If the DT state is different than the CLK state then
// the encoder is rotating CCW so decrement
if (digitalRead(DT) != currentStateCLK) {
counter --;
currDir ="Backward";
Serial.println((counter));
}
else {
// Encoder is rotating CW so increment
counter ++;
currDir ="Forward";
Serial.println((counter));
}
angle = counter * (360.00 / clickNum);
if (currDir != prevDir){
startTime = currMillis;
angAcceli = getAngAccel();
finalTime = startTime;
}
else if (counter == (calTrigger/2)){
angAccelm = getAngAccel();
}
else if (counter <= (calTrigger)){
finalTime = currMillis;
angAccelf = getAngAccel();
}
else if (btDur >= pauseDuration) {
Serial.println(F("Please try again..."));
counter = (trigger + 1);
resetCounter();
}
if (counter >= calTrigger)
{
avgAngAccel = (angAcceli + angAccelf + angAccelm) / 3;
angVelf = getAngVel(avgAngAccel, angle);
maxUserSpeed = getLinVel(angVelf, mainWheelRadiusM);
mode = "Drive";
}
else if (counter <= -calTrigger){
Serial.println(F("Please try again. Push Forward."));
}
else{
Serial.println(F("Keep going"));
}
// Put in a slight delay to help debounce the reading
delay(1);
// printREData(currDir, angle);
resetCounter();
// Remember last CLK state
lastStateCLK = currentStateCLK;
prevDir = currDir;
}
}
void calibration() {
/*Serial.println(F("Calibrating MPU, Keep still in"));
for (int i = 5; i >= 0; --i){
Serial.println(F(i));
delay(1000);
}
calibrateMPU();
*/
Serial.println(F("Please Push Forward as hard as you can..."));
while (mode == "Calibration"){
calibrateEncoders();
}
}
void readTime(){
// Read time
currMillis = millis();
btDur = currMillis - finalTime;
currDur = currMillis - startTime;
totDur = finalTime - startTime;
}
void willMotorStart(){
readTime();
// Read the current state of CLK
currentStateCLK = digitalRead(CLK);
// If last and current state of CLK are different, then pulse occurred
// React to only 1 state change to avoid double count
if (currentStateCLK != lastStateCLK && currentStateCLK == 1){
// If the DT state is different than the CLK state then
// the encoder is rotating CCW so decrement
if (digitalRead(DT) != currentStateCLK) {
counter --;
currDir ="Backward";
}
else {
// Encoder is rotating CW so increment
counter ++;
currDir ="Forward";
}
angle = counter * (360.00 / clickNum);
if (currDir != prevDir){
startTime = currMillis;
angAcceli = getAngAccel();
finalTime = startTime;
}
else if (counter == abs(trigger/2)){
angAccelm = getAngAccel();
}
else if (counter <= abs(trigger)){
finalTime = currMillis;
angAccelf = getAngAccel();
}
else if (btDur >= pauseDuration) {
currDir = 'Idle';
resetCounter();
}
if (counter >= trigger)
{
startMotor();
avgAngAccel = (angAcceli + angAccelf + angAccelm) / 3;
angVelf = getAngVel(avgAngAccel, angle);
currUserSpeed = getLinVel(angVelf, mainWheelRadiusM);
speedRatio = currUserSpeed / maxUserSpeed;
currMotorSpeed = speedRatio * maxMotorSpeed;
}
else if (counter <= -trigger){
startMotor();
angVelf = getAngVel(abs(avgAngAccel), abs(angle));
currUserSpeed = getLinVel(angVelf, mainWheelRadiusM);
speedRatio = currUserSpeed / maxUserSpeed;
currMotorSpeed = speedRatio * maxMotorSpeed;
}
else{
Serial.println(F("Motor Off"));
}
// Put in a slight delay to help debounce the reading
delay(1);
// printREData(currDir, angle);
resetCounter();
}
// Remember last CLK state
lastStateCLK = currentStateCLK;
prevDir = currDir;
}
void startMotor(){
Serial.print(F("Motor started "));
Serial.println((currDir));
Serial.print(F("Motor speed: "));
Serial.println((currMotorSpeed));
delay(duration);
resetCounter();
}
// Select I2C BUS
void TCA9548A(uint8_t bus){
Wire.beginTransmission(0x70); // TCA9548A address
Wire.write(1 << bus); // send byte to select bus
Wire.endTransmission();
Serial.println(bus);
}
void setup() {
// Setup Serial Monitor
Serial.begin(9600);
Wire.begin();
// Set encoder pins as inputs
pinMode(CLK,INPUT);
pinMode(DT,INPUT);
calib1.begin();
Serial.println(F("pre SCREEN"));
// Initialize screen
TCA9548A(0);
if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
Serial.println(F("SSD1306 allocation failed"));
for(;;);
}
// Clear the buffer
display.clearDisplay();
display.display();
delay(2000);
display.clearDisplay();
Serial.println(F("PreMPU"));
// Initialize mpu;
TCA9548A(1);
if (!mpu.begin()) {
Serial.println(F("Failed to find MPU6050 chip"));
while (1) {
delay(10);
}
}
// Read the initial state of CLK
lastStateCLK = digitalRead(CLK);
Serial.println(F("MPU6050 Found!"));
//Set accelerometer range and filter bandwidth (optional, based on your requirements)
mpu.setAccelerometerRange(MPU6050_RANGE_8_G);
mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);
mpu.setGyroStandby(true, true, true);
mpu.setTemperatureStandby(true);
mode = "Calibration";
//calibration();
}
void loop() {
mode = "Drive";
readTime();
willMotorStart();
if (calib1.pressed()){
mode = "Calibration";
calibration();
}
}