1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
#define LGFX_AUTODETECT
#define LGFX_USE_V1

#include <M5Stack.h>
#include "MPU6050_6Axis_MotionApps20.h"
#include <LovyanGFX.hpp>
#include <LGFX_AUTODETECT.hpp>
MPU6050 mpu;

static LGFX lcd;
static LGFX_Sprite canvas(&lcd);

// MPU control/status vars
uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
uint8_t fifoBuffer[64]; // FIFO storage buffer

float GYdegree = 0.0F;
int yaw = 0;

// orientation/motion vars
Quaternion q;           // [w, x, y, z]         quaternion container
VectorFloat gravity;    // [x, y, z]            gravity vector
float ypr[3];           // [roll, pitch, yaw]   roll/pitch/yaw container and gravity vector

void setupMPU() {
  Wire.begin();
  Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties
  mpu.initialize();
  devStatus = mpu.dmpInitialize();

  // supply your own gyro offsets here, scaled for min sensitivity
  mpu.setXAccelOffset(-2634);
  mpu.setYAccelOffset(-38);
  mpu.setZAccelOffset(472);
  mpu.setXGyroOffset(203);
  mpu.setYGyroOffset(54);
  mpu.setZGyroOffset(28);

  // make sure it worked (returns 0 if so)
  if (devStatus == 0) {
    // Calibration Time: generate offsets and calibrate our MPU6050
    mpu.CalibrateAccel(6);
    mpu.CalibrateGyro(6);
    mpu.setDMPEnabled(true);
    packetSize = mpu.dmpGetFIFOPacketSize();
  } else {
    Serial.print("DMP Initialization failed.");
  }
}

void getYawPitchRoll() {
  if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer)) { // Get the Latest packet
    mpu.dmpGetQuaternion(&q, fifoBuffer);
    mpu.dmpGetGravity(&gravity, &q);
    mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
    Serial.println(int(ypr[0] * 180 / M_PI));
  }
}

int GetGyro()   //yaw角のみ角度の取得
{
    getYawPitchRoll();
    yaw = int(ypr[0] * 180 / M_PI);
    return (int)yaw;
}

void DrawGyro()
{
  int gyro = GetGyro();
  M5.Lcd.clear();
  while (true)    //LGFXを用いて円上に現在の角度の点を表示
  { 
    M5.update();
    canvas.fillScreen(BLACK);
    gyro = GetGyro();
    GYdegree = (gyro-90) / (180 / PI);
    canvas.drawCircle(160, 120, 80, WHITE);
    canvas.fillCircle(160 + 80 * cos(GYdegree), 120 + 80 * sin(GYdegree), 10, GREEN);
    canvas.setCursor(160, 0);
    canvas.printf("%4d", gyro);
    canvas.pushSprite(0,0);
  }
}

void setup() {
  M5.begin();
  M5.Power.begin();
  Serial.begin(115200);
  M5.Lcd.fillScreen(BLACK); M5.Lcd.setTextColor(WHITE , BLACK); M5.Lcd.setTextSize(2);  //M5StackのLCDの設定
  lcd.init(); lcd.setRotation(1); canvas.setColorDepth(8); canvas.setTextWrap(false); canvas.setTextSize(2); canvas.createSprite(lcd.width(), lcd.height());   //LGFXの初期設定
  setupMPU();
}

void loop() {
  DrawGyro();
}