mirror of
https://gitee.com/peng_zhihui/HoloCubic
synced 2025-11-04 21:19:40 +08:00
85 lines
1014 B
C++
85 lines
1014 B
C++
#include "imu.h"
|
|
|
|
|
|
int flag;
|
|
extern Pixel rgb;
|
|
|
|
void IMU::init()
|
|
{
|
|
Wire.begin(IMU_I2C_SDA, IMU_I2C_SCL);
|
|
Wire.setClock(400000);
|
|
while (!imu.testConnection());
|
|
imu.initialize();
|
|
}
|
|
|
|
void IMU::update(int interval)
|
|
{
|
|
imu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
|
|
|
|
//Serial.print(gx);
|
|
//Serial.print(" ");
|
|
//Serial.print(gy);
|
|
//Serial.print(" ");
|
|
//Serial.print(gz);
|
|
//Serial.println(" ");
|
|
|
|
if (millis() - last_update_time > interval)
|
|
{
|
|
if (ay > 3000 && flag)
|
|
{
|
|
encoder_diff--;
|
|
flag = 0;
|
|
}
|
|
else if (ay < -3000 && flag)
|
|
{
|
|
encoder_diff++;
|
|
flag = 0;
|
|
}
|
|
else
|
|
{
|
|
flag = 1;
|
|
}
|
|
|
|
if (ax > 10000)
|
|
{
|
|
encoder_state = LV_INDEV_STATE_PR;
|
|
}
|
|
else
|
|
{
|
|
encoder_state = LV_INDEV_STATE_REL;
|
|
}
|
|
|
|
last_update_time = millis();
|
|
}
|
|
}
|
|
|
|
int16_t IMU::getAccelX()
|
|
{
|
|
return ax;
|
|
}
|
|
|
|
int16_t IMU::getAccelY()
|
|
{
|
|
return ay;
|
|
}
|
|
|
|
int16_t IMU::getAccelZ()
|
|
{
|
|
return az;
|
|
}
|
|
|
|
int16_t IMU::getGyroX()
|
|
{
|
|
return gx;
|
|
}
|
|
|
|
int16_t IMU::getGyroY()
|
|
{
|
|
return gy;
|
|
}
|
|
|
|
int16_t IMU::getGyroZ()
|
|
{
|
|
return gz;
|
|
}
|