show mpu6050 angle on oled
// I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class
// 10/7/2011 by Jeff Rowberg <jeff@rowberg.net>
// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
//
// Changelog:
// 2013-05-08 - added multiple output formats
// - added seamless Fastwire support
// 2011-10-07 - initial release
// I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files
// for both classes must be in the include path of your project
#include "I2Cdev.h"
#include "MPU6050.h"
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#define OLED_RESET 4
Adafruit_SSD1306 display(OLED_RESET);
#define NUMFLAKES 10
#define XPOS 0
#define YPOS 1
#define DELTAY 2
// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
// is used in I2Cdev.h
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif
// class default I2C address is 0x68
// specific I2C addresses may be passed as a parameter here
// AD0 low = 0x68 (default for InvenSense evaluation board)
// AD0 high = 0x69
MPU6050 accelgyro;
//MPU6050 accelgyro(0x69); // <-- use for AD0 high
int16_t ax, ay, az;
int16_t gx, gy, gz;
#define LOGO16_GLCD_HEIGHT 32
#define LOGO16_GLCD_WIDTH 32
unsigned char bmp[] =
{
0x0F, 0x0F, 0x0F, 0x0F,
0x0F, 0x0F, 0x0F, 0x0F,
0x0F, 0x0F, 0x0F, 0x0F,
0x0F, 0x0F, 0x0F, 0x0F,
0x0F, 0x0F, 0x0F, 0x0F,
0x0F, 0x0F, 0x0F, 0x0F,
0x0F, 0x0F, 0x0F, 0x0F,
0x0F, 0x0F, 0x0F, 0x0F,
0x0F, 0x0F, 0x0F, 0x0F,
0x0F, 0x0F, 0x0F, 0x0F,
0x0F, 0x0F, 0x0F, 0x0F,
0x0F, 0x0F, 0x0F, 0x0F,
0x0F, 0x0F, 0x0F, 0x0F,
0x0F, 0x0F, 0x0F, 0x0F,
0x0F, 0x0F, 0x0F, 0x0F,
0x0F, 0x0F, 0x0F, 0x0F,
0x0F, 0x0F, 0x0F, 0x0F,
0x0F, 0x0F, 0x0F, 0x0F,
0x0F, 0x0F, 0x0F, 0x0F,
0x0F, 0x0F, 0x0F, 0x0F,
0x0F, 0x0F, 0x0F, 0x0F,
0x0F, 0x0F, 0x0F, 0x0F,
0x0F, 0x0F, 0x0F, 0x0F,
0x0F, 0x0F, 0x0F, 0x0F,
0x0F, 0x0F, 0x0F, 0x0F,
0x0F, 0x0F, 0x0F, 0x0F,
0x0F, 0x0F, 0x0F, 0x0F,
0x0F, 0x0F, 0x0F, 0x0F,
0x0F, 0x0F, 0x0F, 0x0F,
0x0F, 0x0F, 0x0F, 0x0F,
0x0F, 0x0F, 0x0F, 0x0F,
0xFF, 0xFF, 0xFF, 0xFF,
};
// uncomment "OUTPUT_READABLE_ACCELGYRO" if you want to see a tab-separated
// list of the accel X/Y/Z and then gyro X/Y/Z values in decimal. Easy to read,
// not so easy to parse, and slow(er) over UART.
#define OUTPUT_READABLE_ACCELGYRO
// uncomment "OUTPUT_BINARY_ACCELGYRO" to send all 6 axes of data as 16-bit
// binary, one right after the other. This is very fast (as fast as possible
// without compression or data loss), and easy to parse, but impossible to read
// for a human.
//#define OUTPUT_BINARY_ACCELGYRO
#define LED_PIN 13
bool blinkState = false;
void setup() {
// join I2C bus (I2Cdev library doesn't do this automatically)
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
Wire.begin();
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
Fastwire::setup(400, true);
#endif
// initialize serial communication
// (38400 chosen because it works as well at 8MHz as it does at 16MHz, but
// it's really up to you depending on your project)
Serial.begin(38400);
// initialize device
Serial.println("Initializing I2C devices...");
accelgyro.initialize();
// verify connection
Serial.println("Testing device connections...");
Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
// use the code below to change accel/gyro offset values
// configure Arduino LED for
pinMode(LED_PIN, OUTPUT);
// by default, we'll generate the high voltage from the 3.3v line internally! (neat!)
display.begin(SSD1306_SWITCHCAPVCC, 0x3C); // initialize with the I2C addr 0x3D (for the 128x64)
// init done
// Show image buffer on the display hardware.
// Since the buffer is intialized with an Adafruit splashscreen
// internally, this will display the splashscreen.
display.display();
delay(200);
// Clear the buffer.
display.clearDisplay();
display.setTextSize(1);
display.setTextColor(WHITE);
}
/********MP6050相关***************/
float timeChange = 10; //滤波法采样时间间隔毫秒
float dt = timeChange * 0.001; //注意：dt的取值为滤波器采样时间
//MPU6050 Mpu6050;//命名 陀螺仪类：Mpu6050
//int16_t ax, ay, az, gx, gy, gz;//陀螺仪原始数据 3个加速度+3个角速度
float f_angle, angleA, omega, gyroGz, control_v;
float angle0 = 1.80, gy1 = 30; //因重心导致角度偏置；陀螺仪偏移量
int rate = 200;
void loop() {
// read raw accel/gyro measurements from device
//accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);//读取原始6个数据
// val = map(ay,-17000,17000,0,179);
float ax1 = map(ax,-17000,17000,0,179);
float ay1 = map(ay,-17000,17000,0,179);
float az1 = map(az,-17000,17000,0,179);
display.clearDisplay();
display.setCursor(0, 0);
display.print("ax1:");
display.println(ax1);
display.print("ay1:");
display.println(ay1);
display.print("az1:");
display.println(az1);
unsigned char y = 0;
y =AddYPoint(ax1 , 179, 0);
display.setCursor(32, y+32);
display.print("x");
y =AddYPoint(ay1 , 179, 0);
display.setCursor(40, y+32);
display.print("y");
y =AddYPoint(az1 , 179, 0);
display.setCursor(48, y+32);
display.print("z");
display.drawBitmap(0, 32, bmp, LOGO16_GLCD_WIDTH, LOGO16_GLCD_HEIGHT, WHITE);
display.display();
delay(1);
ScrollLeft() ;
}
void ScrollLeft()
{
//
//display.drawBitmap(0, 32, bmp, LOGO16_GLCD_WIDTH, LOGO16_GLCD_HEIGHT, WHITE);
unsigned char totalBytes = LOGO16_GLCD_WIDTH * LOGO16_GLCD_HEIGHT / 8;
unsigned char oneRawNByte = LOGO16_GLCD_WIDTH / 8;
for (unsigned char i = 0; i < totalBytes; i++)
{
bmp[i] = bmp[i] << 1;
if (((i + 1) % oneRawNByte) > 0)
{
bmp[i] = bmp[i] | (bmp[i + 1] >> 7);
}
}
}
void AddPoint(unsigned char x, unsigned char y)
{
unsigned int mapIndex = LOGO16_GLCD_WIDTH * y + x;
unsigned char mapByteIndex = mapIndex / 8;
unsigned char bitIndex = mapIndex % 8;
bmp[mapByteIndex] = bmp[mapByteIndex] | (1 << bitIndex);
}
unsigned char AddYPoint(long value , int maxY, int minY)
{
unsigned char y = LOGO16_GLCD_HEIGHT - 1 - ((value-minY) * LOGO16_GLCD_HEIGHT / (maxY - minY));
AddPoint(LOGO16_GLCD_WIDTH - 1, y);
return y;
}
void ClearMap()
{
unsigned char totalBytes = LOGO16_GLCD_WIDTH * LOGO16_GLCD_HEIGHT / 8;
for (unsigned char i = 0; i < totalBytes; i++)
{
bmp[i] = 0;
}
}
留言
張貼留言