Reputation: 1
In my project, I'm building a smart pen. I'm working on integrating a BMI160 accelerometer and gyro sensor via I2C. The goal is to combine the data from these sensors and translate it into mouse movements.
I used examples provided by the BMI160 library and adjusted them to fit my smart pen project.
I've tried running this same code on my ESP32 board and it works perfectly. However, when I attempt to run it on the ESP32 C3 Super Mini model, some issues arise.
#include <BMI160Gen.h>
#include <Kalman.h> // Source: https://github.com/TKJElectronics/KalmanFilter
#include <BleMouse.h>
#define DPS 1
#define RESTRICT_PITCH // Comment out to restrict roll to ±90deg instead - please read: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf
#define SENSITIVITY 16384
#define G_UNIT 9.822604363512355
Kalman kalmanX; // Create the Kalman instances
Kalman kalmanY;
BleMouse bleMouse("SmartPen","Manufacture",69);
/* IMU Data */
int accX, accY, accZ;
int gyroX, gyroY, gyroZ;
int16_t tempRaw;
double gyroXangle, gyroYangle; // Angle calculate using the gyro only
double compAngleX, compAngleY; // Calculated angle using a complementary filter
double kalAngleX, kalAngleY; // Calculated angle using a Kalman filter
double offSetAccX;
double offSetAccY;
double offSetAccZ;
uint32_t timer;
const int select_pin = 10;
const int i2c_addr = 0x69;
float AcX=0;
float AcY=0;
float AcZ=0;
float VcX;
float VcY;
float VcZ;
void setup() {
Serial.begin(115200); // inicializa a comunicação Serial
while (!Serial); // espera a porta serial abrir
Serial.println("Initializing BLEMouse...");
bleMouse.begin();
Serial.println("BLEMouse initialized successfully.");
// inicializa o sensor BMI160
BMI160.begin(BMI160GenClass::I2C_MODE, i2c_addr);
BMI160.setGyroRange(250);
BMI160.setAccelerometerRange(2);
delay(500);
BMI160.readGyro(gyroX, gyroY, gyroZ);
BMI160.readAccelerometer(accX, accY, accZ);
#ifdef RESTRICT_PITCH // Eq. 25 and 26
double roll = atan2(accY, accZ) * RAD_TO_DEG;
double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
#else // Eq. 28 and 29
double roll = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;
double pitch = atan2(-accX, accZ) * RAD_TO_DEG;
#endif
kalmanX.setAngle(roll); // Set starting angle
kalmanY.setAngle(pitch);
gyroXangle = roll;
gyroYangle = pitch;
compAngleX = roll;
compAngleY = pitch;
timer = micros();
if(accX>16000){offSetAccX=accX-16384;}else{offSetAccX=accX;}
if(accY>16000){offSetAccY=accY-16384;}else{offSetAccY=accY;}
if(accZ>16000){offSetAccZ=accZ-16384;}else{offSetAccZ=accZ;}
}
void loop() {
// int gx, gy, gz; // valores brutos do giroscópio
// int ax, ay, az; // valores brutos do acelerômetro
#if 1
if(bleMouse.isConnected()) {
bleMouse.move((VcX*DPS),(VcY*DPS),0,0);
}
#endif
// lê as medições brutas do giroscópio e acelerômetro do sensor
BMI160.readGyro(gyroX, gyroY, gyroZ);
BMI160.readAccelerometer(accX, accY, accZ);
double dt = (double)(micros() - timer) / 1000000; // Calculate delta time
timer = micros();
// Source: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf eq. 25 and eq. 26
// atan2 outputs the value of -π to π (radians) - see http://en.wikipedia.org/wiki/Atan2
// It is then converted from radians to degrees
#ifdef RESTRICT_PITCH // Eq. 25 and 26
double roll = atan2(accY, accZ) * RAD_TO_DEG;
double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
#else // Eq. 28 and 29
double roll = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;
double pitch = atan2(-accX, accZ) * RAD_TO_DEG;
#endif
double gyroXrate = gyroX / 131.0; // Convert to deg/s
double gyroYrate = gyroY / 131.0; // Convert to deg/s
#ifdef RESTRICT_PITCH
// This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
if ((roll < -90 && kalAngleX > 90) || (roll > 90 && kalAngleX < -90)) {
kalmanX.setAngle(roll);
compAngleX = roll;
kalAngleX = roll;
gyroXangle = roll;
} else
kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter
if (abs(kalAngleX) > 90)
gyroYrate = -gyroYrate; // Invert rate, so it fits the restriced accelerometer reading
kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt);
#else
// This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
if ((pitch < -90 && kalAngleY > 90) || (pitch > 90 && kalAngleY < -90)) {
kalmanY.setAngle(pitch);
compAngleY = pitch;
kalAngleY = pitch;
gyroYangle = pitch;
} else
kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt); // Calculate the angle using a Kalman filter
if (abs(kalAngleY) > 90)
gyroXrate = -gyroXrate; // Invert rate, so it fits the restriced accelerometer reading
kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter
#endif
//gyroXangle += gyroXrate * dt; // Calculate gyro angle without any filter
//gyroYangle += gyroYrate * dt;
gyroXangle += kalmanX.getRate() * dt; // Calculate gyro angle using the unbiased rate
gyroYangle += kalmanY.getRate() * dt;
//gyroYangle += kalmanZ.getRate() * dt;
compAngleX = 0.93 * (compAngleX + gyroXrate * dt) + 0.07 * roll; // Calculate the angle using a Complimentary filter
compAngleY = 0.93 * (compAngleY + gyroYrate * dt) + 0.07 * pitch;
// Reset the gyro angle when it has drifted too much
if (gyroXangle < -180 || gyroXangle > 180)
gyroXangle = kalAngleX;
if (gyroYangle < -180 || gyroYangle > 180)
gyroYangle = kalAngleY;
AcX = (float(accX - offSetAccX) / SENSITIVITY) * G_UNIT;
AcY = (float(accY - offSetAccY) / SENSITIVITY) * G_UNIT;
AcZ = (float(accZ - offSetAccZ) / SENSITIVITY) * G_UNIT;
#if 1
VcX += AcX * dt;
VcY += AcY * dt;
VcZ += AcZ * dt;
if(VcX>3){VcX=3;}
if(VcX<-3){VcX=-3;}
if(VcY>3){VcY=3;}
if(VcY<-3){VcY=-3;}
if(VcZ>3){VcZ=3;}
if(VcZ<-3){VcZ=-3;}
#endif
/* Print Data */
#if 1// Set to 1 to activate
Serial.print("accX:");Serial.print(AcX,1); Serial.print("\t");
Serial.print("accY:");Serial.print(AcY,1); Serial.print("\t");
Serial.print("accZ:");Serial.print(AcZ,6); Serial.print("\t");
#endif
#if 0 // Set to 1 to activate
Serial.print("gyroX:");Serial.print(gyroX); Serial.print("\t");
Serial.print("gyroY:"); Serial.print(gyroY); Serial.print("\t");
Serial.print("gyroZ:");Serial.print(gyroZ); Serial.print("\t");
Serial.print("\t");
#endif
#if 0
Serial.print("roll:");Serial.print(roll); Serial.print("\t");
//Serial.print("gyroXangle:");Serial.print(gyroXangle); Serial.print("\t");
//Serial.print("compAngleX:");Serial.print(compAngleX); Serial.print("\t");
//Serial.print("kalAngleX:\t");Serial.print(kalAngleX); Serial.print("\t");
Serial.print("\t");
Serial.print("pitch:");Serial.print(pitch); Serial.print("\t");
//Serial.print("gyroYangle:");Serial.print(gyroYangle); Serial.print("\t");
//Serial.print("compAngleY:");Serial.print(compAngleY); Serial.print("\t");
//Serial.print("kalAngleY:\t");Serial.print(kalAngleY); Serial.print("\t");
#endif
Serial.print("\t");
#if 0
Serial.print("VcX:");Serial.print(VcX,1); Serial.print("\t");
Serial.print("VcY:");Serial.print(VcY,1); Serial.print("\t");
Serial.print("VcZ:");Serial.print(VcZ,6); Serial.print("\t");
#endif
Serial.print("\r\n");
delay(2);
}
The code stops at 'bleMouse.begin();'.
If anyone has any tips or suggestions beyond this specific issue, I would greatly appreciate your input. Thank you in advance for any assistance you can provide!
Upvotes: 0
Views: 178