RogerCruz
RogerCruz

Reputation: 1

ESP32 C3 Super Mini can't start BLE Mouse

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();'.

board settings

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

Answers (0)

Related Questions