bluetoothジャイロマウスのarduinoコード

ESP_Gyro_mouse.cpp

#include  <Wire.h>    //esp32 SDA=21,SCL=22
#include  <BleMouse.h>
BleMouse bleMouse;

#define  MPU6050_ADDR         0x68 // MPU-6050 device address
#define  MPU6050_SMPLRT_DIV   0x19 // MPU-6050 register address
#define  MPU6050_CONFIG       0x1a
#define  MPU6050_GYRO_CONFIG  0x1b
#define  MPU6050_ACCEL_CONFIG 0x1c
#define  MPU6050_WHO_AM_I     0x75
#define  MPU6050_PWR_MGMT_1   0x6b

double offsetX = 0, offsetY = 0, offsetZ = 0;
double gyro_angle_x = 0, gyro_angle_y = 0, gyro_angle_z = 0;
float angleX, angleY, angleZ;
float interval, preInterval;
float acc_x, acc_y, acc_z, acc_angle_x, acc_angle_y;
float gx, gy, gz, dpsX, dpsY, dpsZ;
float x_kand=0.8;
float y_kand=0.8;
float sc_kand=0.05;
int i1=0, i2=0;

void culcRotation();
//I2c書き込み
void writeMPU6050(byte reg, byte data) {
 Wire.beginTransmission(MPU6050_ADDR);
 Wire.write(reg);   
 Wire.write(data);
 Wire.endTransmission();
}
//i2C読み込み
byte readMPU6050(byte reg) {
 Wire.beginTransmission(MPU6050_ADDR);
 Wire.write(reg);
 Wire.endTransmission(true);
 Wire.requestFrom(MPU6050_ADDR, 1/*length*/); 
 byte data =  Wire.read();
 return data;
}
void setup() {
 pinMode(16,INPUT);
 pinMode(17,INPUT);
 pinMode(18,INPUT);
 
 Wire.begin();
 Wire.beginTransmission(MPU6050_ADDR);
 Wire.write(0x6B); // PWR_MGMT_1 register
 Wire.write(0); // set to zero (wakes up the MPU-6050)
 Wire.endTransmission(true);
 
 Serial.begin(9600);
 bleMouse.begin();
 
 delay(100);
 
 //正常に接続されているかの確認
 if (readMPU6050(MPU6050_WHO_AM_I) != 0x68) {
   Serial.println("\nWHO_AM_I error.");
   while (true) ;
 }
 //設定を書き込む
 writeMPU6050(MPU6050_SMPLRT_DIV, 0x00);   // sample rate: 8kHz/(7+1) = 1kHz
 writeMPU6050(MPU6050_CONFIG, 0x00);       // disable DLPF, gyro output rate = 8kHz
 writeMPU6050(MPU6050_GYRO_CONFIG, 0x08);  // gyro range: ±500dps
 writeMPU6050(MPU6050_ACCEL_CONFIG, 0x00); // accel range: ±2g
 writeMPU6050(MPU6050_PWR_MGMT_1, 0x01);   // disable sleep mode, PLL with X gyro
 //キャリブレーション
 Serial.print("Calculate Calibration");
 for(int i = 0; i < 3000; i++){
   
   int16_t raw_acc_x, raw_acc_y, raw_acc_z, raw_t, raw_gyro_x, raw_gyro_y, raw_gyro_z ;
   
   Wire.beginTransmission(MPU6050_ADDR);
   Wire.write(0x3B);
   Wire.endTransmission(false);
   Wire.requestFrom(MPU6050_ADDR, 14, true);
 
   raw_acc_x = Wire.read() << 8 | Wire.read();
   raw_acc_y = Wire.read() << 8 | Wire.read();
   raw_acc_z = Wire.read() << 8 | Wire.read();
   raw_t = Wire.read() << 8 | Wire.read();
   raw_gyro_x = Wire.read() << 8 | Wire.read();
   raw_gyro_y = Wire.read() << 8 | Wire.read();
   raw_gyro_z = Wire.read() << 8 | Wire.read();
   dpsX = ((float)raw_gyro_x) / 65.5;
   dpsY = ((float)raw_gyro_y) / 65.5;
   dpsZ = ((float)raw_gyro_z) / 65.5;
   offsetX += dpsX;
   offsetY += dpsY;
   offsetZ += dpsZ;
   if(i % 1000 == 0){
     Serial.print(".");
   }
 }
 Serial.println();
 offsetX /= 3000;
 offsetY /= 3000;
 offsetZ /= 3000;
 Serial.print("offsetX : ");
 Serial.println(offsetX);
 Serial.print("offsetY : ");
 Serial.println(offsetY);
 Serial.print("offsetZ : ");
 Serial.println(offsetZ);
}
void loop() {
 calcRotation();
 Serial.print("  Z : ");
 Serial.print(dpsZ);
 Serial.print("  Y : ");
 Serial.print(dpsY-3);
 Serial.println(""); // 改行
 if(bleMouse.isConnected()) {     //接続状況確認
   if(touchRead(T0)>30){
      bleMouse.move(-(dpsZ)*x_kand, (dpsY-3)*y_kand, 0);
   }
   if(digitalRead(16)==0){
      bleMouse.press();
      delay(10);
      i1=1;
   }
   if(i1==1 && digitalRead(16)==1){
      bleMouse.release();
      i1=0;
   }
   if(digitalRead(17)==0){
      bleMouse.press(MOUSE_RIGHT);
      delay(10);
      i2=1;
   }
   if(i2==1 && digitalRead(17)==1){
      bleMouse.release(MOUSE_RIGHT);
      i2=0;
   }
   if(digitalRead(18)==0){
      bleMouse.move(0,0, (dpsY-3)*sc_kand);
   }
 }
}
//加速度、ジャイロから角度を計算
void calcRotation(){
 int16_t raw_acc_x, raw_acc_y, raw_acc_z, raw_t, raw_gyro_x, raw_gyro_y, raw_gyro_z ;
 
 //レジスタアドレス0x3Bから、計14バイト分のデータを出力するようMPU6050へ指示
 Wire.beginTransmission(MPU6050_ADDR);
 Wire.write(0x3B);
 Wire.endTransmission(false);
 Wire.requestFrom(MPU6050_ADDR, 14, true);
 
 //出力されたデータを読み込み、ビットシフト演算
 raw_acc_x = Wire.read() << 8 | Wire.read();
 raw_acc_y = Wire.read() << 8 | Wire.read();
 raw_acc_z = Wire.read() << 8 | Wire.read();
 raw_t = Wire.read() << 8 | Wire.read();
 raw_gyro_x = Wire.read() << 8 | Wire.read();
 raw_gyro_y = Wire.read() << 8 | Wire.read();
 raw_gyro_z = Wire.read() << 8 | Wire.read();
 dpsX = ((float)raw_gyro_x) / 65.5; // LSB sensitivity: 65.5 LSB/dps @ ±500dps
 dpsY = ((float)raw_gyro_y) / 65.5;
 dpsZ = ((float)raw_gyro_z) / 65.5;
 
}