(로봇카/esp32) 여러 센서 정보를 OLED에 출력하기(MPU6050, INA219, HC-SR04)

  // for esp32: oled display (mpu6050 + ina219 + IR sensor)

// for MPU6050_DMP6 library
#include "MPU6050_setup.h" // always first in the headers

// for OLED 1.3" display
#include <Adafruit_GFX.h>
#include <Adafruit_SH110X.h>
#define i2c_Address 0x3c
Adafruit_SH1106G display = Adafruit_SH1106G(128, 64, &Wire, -1);

// for INA219
#include <Adafruit_INA219.h>
Adafruit_INA219 ina219;


// for HC-SR04 IR sensor
const int echoPin = 19;
const int trigPin = 18;
#define SOUND_SPEED 0.034
long duration;
float distance;

// for INA219
float busvoltage = 0;
float current_mA = 0;

void setup() {
  MPU6050_setup(); // always first in the setup()

  // init HC-SR04 IR sensor
  Serial.begin(115200);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);

  // init OLED 1.3" display
  delay(250); // wait for OLED power up
  display.begin(i2c_Address, true);
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SH110X_WHITE);
  if (!ina219.begin()) {
    //display.clearDisplay();
    display.setCursor(0, 0);
    Serial.println("Failed to find INA219 chip");
  }
}

// values for MPU6050
float prev_yaw = 0.0;
unsigned long t = 0;
const unsigned int checkInterval = 30;
const unsigned int ignoreChangeNoise = 80;  // ignored degree change. sudden a lot degree change in a very short time can be a noise.

void loop() {
  
  // get data from MPU6050_DMP6
  mpuDmpGet();

  Serial.print("ypr  ");
  float yaw = ypr[0] * 180 / M_PI; // 라디안을 360도 각도로 변환
  if (yaw < 0) yaw = 360 + yaw; // 마이너스 값이면 360을 더해서 양각으로 만듦
  
  //갑작스런 수치 변화를 노이즈로 간주하고 무시
  if (abs(yaw - prev_yaw) < ignoreChangeNoise) {
    prev_yaw = yaw;  // yaw값이 정상이라면 갱신
  } else if (abs(yaw - prev_yaw - 360) < ignoreChangeNoise) { // 360 라인을 넘어서는 바람에 yaw값이 커진 것이라면; 반시계 방향
    prev_yaw = yaw;  // yaw값이 정상이라면 갱신
  } else if (abs(yaw - prev_yaw + 360) < ignoreChangeNoise){ // 360 라인을 넘어서는 바람에 yaw값이 커진 것이라면; 시계 방향
    prev_yaw = yaw;
  } else {                                    
    yaw = prev_yaw;  // 비정상적인 경우에는 갱신하지 않음, 이전값 고수
  }
  Serial.print(yaw);

  Serial.print("  ");
  float pitch = ypr[1] * 180 / M_PI; // 라디안을 360도 각도로 변환
  if (pitch < 0) pitch = 360.0 + pitch;
  Serial.print(pitch);   
  
  Serial.print("  ");
  float roll = ypr[2] * 180 / M_PI; // 라디안을 360도 각도로 변환
  if (roll < 0) roll = 360.0 + roll;
  Serial.print(roll);  
  
  // get data from HC-SR04 IR sensor 
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  duration = pulseIn(echoPin, HIGH);
  distance = duration * SOUND_SPEED / 2;

  Serial.print("\tDistance (cm): ");
  Serial.print(distance);
  Serial.print("\t");
  
  // display all data on the OLED display
  display.clearDisplay();
  display.setCursor(0, 0);
  // first yaw, pitch, roll
  display.print(yaw);
  display.print(" ");
  display.print(pitch);
  display.print(" ");
  display.println(roll);
  // second the distance from IR sensor
  display.print("distance: ");
  display.print(distance);
  display.print(" cm   ");
  // display INA219 info.
  busvoltage = ina219.getBusVoltage_V();
  current_mA = ina219.getCurrent_mA();
  Serial.print(busvoltage); Serial.print(" V  ");
  Serial.print(current_mA); Serial.println(" mA");
  display.print(busvoltage); display.print(" V  ");
  display.print(current_mA); display.println(" mA");

  // update the screen
  display.display();


  /*
  //해당 각도로 이동하는 코드
  if (millis() - t > checkInterval) {
    t = millis();
    myStepper.dstep_coor(round(yaw));
  }
  */
  delay(100);
}