Khoa Bui Anh
drum team
Dependencies: BLE_API X_NUCLEO_IDB0XA1 mbed
Fork of
BLE_HeartRate_IDB0XA1
by 
ST
Diff: main.cpp
- Revision:
- 22:65f63e2d06bd
- Parent:
- 21:0e7c08f5386f
--- a/main.cpp Wed Oct 05 09:16:58 2016 +0000
+++ b/main.cpp Sun Oct 30 00:13:37 2016 +0000
@@ -15,12 +15,43 @@
*/
#include "mbed.h"
+#include "MPU6050.h"
#include "ble/BLE.h"
#include "ble/services/HeartRateService.h"
+#define ratio 1
+
+int i= 0,j = 0;
+float sum = 0;
+uint32_t sumCount = 0;
+volatile uint8_t hrmCounter;
+
+float central1[3], central2[3];
+float drum1_min[3],drum2_min[3],drum3_min[3],drum4_min[3],drum5_min[3],drum6_min[3],drum7_min[3],drum8_min[3],drum9_min[3],drum10_min[3];
+float drum1_max[3],drum2_max[3],drum3_max[3],drum4_max[3],drum5_max[3],drum6_max[3],drum7_max[3],drum8_max[3],drum9_max[3],drum10_max[3];
+int flag = 0;
+int stt1 = 0, stt2 = 0;
+int drum1_stt1 = 0,drum2_stt1 = 0,drum3_stt1 = 0,drum4_stt1 = 0,drum5_stt1 = 0;
+int drum1_stt2 = 0,drum2_stt2 = 0,drum3_stt2 = 0,drum4_stt2 = 0,drum5_stt2 = 0;
+
+
+ InterruptIn mybutton(USER_BUTTON);
+
+ MPU6050 mpu6050;
+
+ MPU6050 mpu6050_2;
+
+ Timer t;
+
+ Serial pc(USBTX, USBRX); // tx, rx
+
+ // VCC, SCE, RST, D/C, MOSI,S CLK, LED
+ //N5110 lcd(PA_8, PB_10, PA_9, PA_6, PA_7, PA_5, PC_7);
+
+void get();
DigitalOut led1(LED1, 1);
-const static char DEVICE_NAME[] = "HRM1";
+const static char DEVICE_NAME[] = "IDB";
static const uint16_t uuid16_list[] = {GattService::UUID_HEART_RATE_SERVICE};
static volatile bool triggerSensorPolling = false;
@@ -63,7 +94,7 @@
ble.gap().onDisconnection(disconnectionCallback);
/* Setup primary service. */
- uint8_t hrmCounter = 60; // init HRM to 60bps
+ uint8_t hrmCounter = 'A'; // init HRM to 60bps
HeartRateService hrService(ble, hrmCounter, HeartRateService::LOCATION_FINGER);
/* Setup advertising. */
@@ -74,25 +105,295 @@
ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
ble.gap().setAdvertisingInterval(1000); /* 1000ms */
ble.gap().startAdvertising();
+
+ pc.baud(9600);
+ //Set up I2C
+ i2c.frequency(400000); // use fast (400 kHz) I2C
+ i2c2.frequency(400000);
+ t.start();
+
+ //lcd.init();
+ //lcd.setBrightness(0.05);
+
+
+ // Read the WHO_AM_I register, this is a good test of communication
+ uint8_t whoami = mpu6050.readByte(MPU6050_ADDRESS, WHO_AM_I_MPU6050); // Read WHO_AM_I register for MPU-6050
+ pc.printf("I AM 0x%x\n\r", whoami); pc.printf("I SHOULD BE 0x68\n\r");
+
+ if (whoami == 0x68) // WHO_AM_I should always be 0x68
+ {
+ pc.printf("MPU6050 is online...");
+ wait(1);
+ //lcd.clear();
+ //lcd.printString("MPU6050 OK", 0, 0);
+
+
+ mpu6050.MPU6050SelfTest(SelfTest); // Start by performing self test and reporting values
+ pc.printf("x-axis self test: acceleration trim within : "); pc.printf("%f", SelfTest[0]); pc.printf("% of factory value \n\r");
+ pc.printf("y-axis self test: acceleration trim within : "); pc.printf("%f", SelfTest[1]); pc.printf("% of factory value \n\r");
+ pc.printf("z-axis self test: acceleration trim within : "); pc.printf("%f", SelfTest[2]); pc.printf("% of factory value \n\r");
+ pc.printf("x-axis self test: gyration trim within : "); pc.printf("%f", SelfTest[3]); pc.printf("% of factory value \n\r");
+ pc.printf("y-axis self test: gyration trim within : "); pc.printf("%f", SelfTest[4]); pc.printf("% of factory value \n\r");
+ pc.printf("z-axis self test: gyration trim within : "); pc.printf("%f", SelfTest[5]); pc.printf("% of factory value \n\r");
+ wait(1);
+
+ if(SelfTest[0] < 1.0f && SelfTest[1] < 1.0f && SelfTest[2] < 1.0f && SelfTest[3] < 1.0f && SelfTest[4] < 1.0f && SelfTest[5] < 1.0f)
+ {
+ mpu6050.resetMPU6050(); // Reset registers to default in preparation for device calibration
+ mpu6050.calibrateMPU6050(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
+ mpu6050.initMPU6050(); pc.printf("MPU6050 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
+
+ /*lcd.clear();
+ lcd.printString("MPU6050", 0, 0);
+ lcd.printString("pass self test", 0, 1);
+ lcd.printString("initializing", 0, 2); */
+ wait(2);
+ }
+ else
+ {
+ pc.printf("Device did not the pass self-test!\n\r");
+
+ /*lcd.clear();
+ lcd.printString("MPU6050", 0, 0);
+ lcd.printString("no pass", 0, 1);
+ lcd.printString("self test", 0, 2);*/
+ }
+ }
+ else
+ {
+ pc.printf("Could not connect to MPU6050: \n\r");
+ pc.printf("%#x \n", whoami);
+
+ /*lcd.clear();
+ lcd.printString("MPU6050", 0, 0);
+ lcd.printString("no connection", 0, 1);
+ lcd.printString("0x", 0, 2); lcd.setXYAddress(20, 2); lcd.printChar(whoami);*/
+
+ while(1) ; // Loop forever if communication doesn't happen
+ }
+
// infinite loop
- while (true) {
+ while (1) {
// check for trigger from periodicCallback()
if (triggerSensorPolling && ble.getGapState().connected) {
triggerSensorPolling = false;
- // Do blocking calls or whatever is necessary for sensor polling.
- // In our case, we simply update the HRM measurement.
- hrmCounter++;
+ // If data ready bit set, all data registers have new data
+ if(mpu6050.readByte(MPU6050_ADDRESS, INT_STATUS) & 0x01) { // check if data ready interrupt
+ mpu6050.readAccelData(accelCount); // Read the x/y/z adc values
+ mpu6050.getAres();
+
+ // Now we'll calculate the accleration value into actual g's
+ ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set
+ ay = (float)accelCount[1]*aRes - accelBias[1];
+ az = (float)accelCount[2]*aRes - accelBias[2];
+
+ ax2 = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set
+ ay2 = (float)accelCount[1]*aRes - accelBias[1];
+ az2 = (float)accelCount[2]*aRes - accelBias[2];
+
+ mpu6050.readGyroData(gyroCount); // Read the x/y/z adc values
+ mpu6050.getGres();
+
+ // Calculate the gyro value into actual degrees per second
+ gx = (float)gyroCount[0]*gRes; // - gyroBias[0]; // get actual gyro value, this depends on scale being set
+ gy = (float)gyroCount[1]*gRes; // - gyroBias[1];
+ gz = (float)gyroCount[2]*gRes; // - gyroBias[2];
+
+ gx2 = (float)gyroCount[0]*gRes; // - gyroBias[0]; // get actual gyro value, this depends on scale being set
+ gy2 = (float)gyroCount[1]*gRes; // - gyroBias[1];
+ gz2 = (float)gyroCount[2]*gRes; // - gyroBias[2];
+
+ tempCount = mpu6050.readTempData(); // Read the x/y/z adc values
+ temperature = (tempCount) / 340. + 36.53; // Temperature in degrees Centigrade
+ }
+
+ Now = t.read_us();
+ deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
+ lastUpdate = Now;
+
+ sum += deltat;
+ sumCount++;
+
+ if(lastUpdate - firstUpdate > 10000000.0f) {
+ beta = 0.04; // decrease filter gain after stabilized
+ zeta = 0.015; // increasey bias drift gain after stabilized
+ }
+
+ // Pass gyro rate as rad/s
+ mpu6050.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f);
+ mpu6050.MadgwickQuaternionUpdate(ax2, ay2, az2, gx2*PI/180.0f, gy2*PI/180.0f, gz2*PI/180.0f);
+
+ // Serial print and/or display at 0.5 s rate independent of data rates
+ delt_t = t.read_ms() - count_mpu;
+ //if (delt_t > 500) { // update LCD once per half-second independent of read rate
+
+ //pc.printf("ax = %f", 1000*ax);
+ //pc.printf(" ay = %f", 1000*ay);
+ //pc.printf(" az = %f mg\n\r", 1000*az);
+
+ //pc.printf("gx = %f", gx);
+ //pc.printf(" gy = %f", gy);
+ //pc.printf(" gz = %f deg/s\n\r", gz);
+
+ //pc.printf(" temperature = %f C\n\r", temperature);
+
+ //pc.printf("q0 = %f\n\r", q[0]);
+ //pc.printf("q1 = %f\n\r", q[1]);
+ //pc.printf("q2 = %f\n\r", q[2]);
+ //pc.printf("q3 = %f\n\r", q[3]);
+
+ //lcd.clear();
+ //lcd.printString("MPU6050", 0, 0);
+ //lcd.printString("x y z", 0, 1);
+ //lcd.setXYAddress(0, 2); lcd.printChar((char)(1000*ax));
+ //lcd.setXYAddress(20, 2); lcd.printChar((char)(1000*ay));
+ //lcd.setXYAddress(40, 2); lcd.printChar((char)(1000*az)); lcd.printString("mg", 66, 2);
+
+
+ // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation.
+ // In this coordinate system, the positive z-axis is down toward Earth.
+ // Yaw is the angle between Sensor x-axis and Earth magnetic North (or true North if corrected for local declination, looking down on the sensor positive yaw is counterclockwise.
+ // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative.
+ // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll.
+ // These arise from the definition of the homogeneous rotation matrix constructed from quaternions.
+ // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be
+ // applied in the correct order which for this configuration is yaw, pitch, and then roll.
+ // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links.
+ yaw = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);
+ pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
+ roll = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
+ pitch *= 180.0f / PI;
+ yaw *= 180.0f / PI;
+ roll *= 180.0f / PI;
+
+ yaw2 = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);
+ pitch2 = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
+ roll2 = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
+ pitch2 *= 180.0f / PI;
+ yaw2 *= 180.0f / PI;
+ roll2 *= 180.0f / PI;
+
+// pc.printf("Yaw, Pitch, Roll: \n\r");
+// pc.printf("%f", yaw);
+// pc.printf(", ");
+// pc.printf("%f", pitch);
+// pc.printf(", ");
+// pc.printf("%f\n\r", roll);
+// pc.printf("average rate = "); pc.printf("%f", (sumCount/sum)); pc.printf(" Hz\n\r");
- // 60 <= HRM bps <= 100
- if (hrmCounter == 100) {
- hrmCounter = 60;
- }
+ //pc.printf("Yaw, Pitch, Roll: %f %f %f\n\r", yaw, pitch, roll);
+ //pc.printf("average rate = %f\n\r", (float) sumCount/sum);
+
+ myled= !myled;
+ count_mpu = t.read_ms();
+ sum = 0;
+ sumCount = 0;
+ if ((yaw < 1) && (yaw > 0) && (flag == 0))
+ {
+
+ central1[0] = yaw;
+ central1[1] = pitch;
+ central1[2] = roll;
+ central2[0] = yaw2;
+ central2[1] = pitch2;
+ central2[2] = roll2;
+
- // update bps
- hrService.updateHeartRate(hrmCounter);
- } else {
+ pc.printf("central x y z : %f %f %f \r\n", central1[0],central1[1],central1[2]);
+ flag = 1;
+ }
+ //if (i == 2000) i = 0;
+ if (flag == 1)
+ {
+ pc.printf("Yaw, Pitch, Roll: %f %f %f\n\r", yaw, pitch, roll);
+ //hrService.updateHeartRate((uint8_t)yaw);
+ //pc.printf("Yaw, Pitch, Roll: %f %f %f\n\r", drum1_min[0], drum1_max[0], drum1_min[2]);
+ switch (stt1)
+ {
+ case 0:
+ pc.printf("%d",stt1);
+ if (/*((yaw > drum1_min[0]) ||*/ (yaw < drum1_max[0]) /*&& (pitch > drum1_min[1]) && (pitch < drum1_max[1]) && (roll > drum1_max[2]) && (roll < drum1_max[2])*/) stt1 = 1;
+ else if (/*(yaw > drum2_min[0]) ||*/ (yaw < drum2_max[0])/* && (pitch > drum2_min[1]) && (pitch < drum2_max[1]) && (roll > drum2_max[2]) && (roll < drum2_max[2])*/) stt1 = 2;
+ else if (/*(yaw > drum3_min[0]) ||*/ (yaw < drum3_max[0])/* && (pitch > drum3_min[1]) && (pitch < drum3_max[1]) && (roll > drum3_max[2]) && (roll < drum3_max[2])*/) stt1 = 3;
+ hrService.updateHeartRate((uint8_t)96);
+ break;
+ case 1:
+ pc.printf("%d",stt1);
+ if (drum1_stt1 == 0)
+ {
+ pc.printf("drum 1_1\r\n");
+ drum1_stt1 = 1;
+ hrService.updateHeartRate((uint8_t)69);
+ }
+ else if ((yaw > drum1_max[0]) || (pitch > drum1_max[1]) /*&& (roll > drum1_max[2])*/) {
+ stt1 = 0 ;
+ drum1_stt1 = 0;
+ pc.printf("up\r\n");
+ }
+ break;
+ case 2:
+ pc.printf("%d",stt1);
+ if (drum2_stt1 == 0)
+ {
+ pc.printf("drum 2_2\r\n");
+ drum1_stt1 = 1;
+ hrService.updateHeartRate((uint8_t)88);
+ }
+ else if ((yaw > drum2_max[0]) && (pitch > drum2_max[1]) /*&& (roll > drum2_max[2])*/) {
+ stt1 = 0 ;
+ drum1_stt1 = 0;
+ pc.printf("up\r\n");
+ }
+ break;
+ case 3:
+ pc.printf("%d",stt1);
+ if (drum3_stt1 == 0)
+ {
+ pc.printf("drum 3_3\r\n");
+ drum3_stt1 = 1;
+ hrService.updateHeartRate((uint8_t)3);
+ }
+ else if ((yaw > drum3_max[0]) && (pitch > drum3_max[1]) /*&& (roll > drum2_max[2])*/) {
+ stt1 = 0 ;
+ drum3_stt1 = 0;
+ pc.printf("up\r\n");
+ }
+ break;
+ case 4:
+ pc.printf("%d",stt1);
+ if (drum4_stt1 == 0)
+ {
+ pc.printf("drum 4_4\r\n");
+ drum4_stt1 = 1;
+ hrService.updateHeartRate((uint8_t)4);
+ }
+ else if ((yaw > drum4_max[0]) && (pitch > drum4_max[1]) /*&& (roll > drum2_max[2])*/) {
+ stt1 = 0 ;
+ drum4_stt1 = 0;
+ pc.printf("up\r\n");
+ }
+ break;
+ case 5:
+ pc.printf("%d",stt1);
+ if (drum5_stt1 == 0)
+ {
+ pc.printf("drum 4_4\r\n");
+ drum5_stt1 = 1;
+ hrService.updateHeartRate((uint8_t)5);
+ }
+ else if ((yaw > drum5_max[0]) && (pitch > drum5_max[1]) /*&& (roll > drum2_max[2])*/) {
+ stt1 = 0 ;
+ drum5_stt1 = 0;
+ pc.printf("up\r\n");
+ }
+ break;
+ };
+}
+ }
+//}
+ else {
ble.waitForEvent(); // low power wait for event
}
}
@@ -101,8 +402,107 @@
int main(void)
{
Ticker ticker;
- ticker.attach(periodicCallback, 1); // blink LED every second
+ ticker.attach(periodicCallback, 0.01); // blink LED every second
+ mybutton.fall(get);
+
BLE::Instance().init(bleInitComplete);
}
+void get()
+{
+ j++;
+ if (j == 1){
+ drum1_min[0] = yaw - ratio;
+ drum1_min[1] = pitch - ratio;
+ drum1_min[2] = roll - ratio;
+
+ drum1_max[0] = yaw + ratio;
+ drum1_max[1] = pitch + ratio;
+ drum1_max[2] = roll + ratio;
+ }
+ else if (j == 2){
+ drum2_min[0] = yaw - ratio;
+ drum2_min[1] = pitch - ratio;
+ drum2_min[2] = roll - ratio;
+
+ drum2_max[0] = yaw + ratio;
+ drum2_max[1] = pitch + ratio;
+ drum2_max[2] = roll + ratio;
+ }
+ else if (j == 3){
+ drum3_min[0] = yaw - ratio;
+ drum3_min[1] = pitch - ratio;
+ drum3_min[2] = roll - ratio;
+
+ drum3_max[0] = yaw + ratio;
+ drum3_max[1] = pitch + ratio;
+ drum3_max[2] = roll + ratio;
+ }
+ else if (j == 4){
+ drum4_min[0] = yaw - ratio;
+ drum4_min[1] = pitch - ratio;
+ drum4_min[2] = roll - ratio;
+
+ drum4_max[0] = yaw + ratio;
+ drum4_max[1] = pitch + ratio;
+ drum4_max[2] = roll + ratio;
+ }
+ else if (j == 5){
+ drum5_min[0] = yaw - ratio;
+ drum5_min[1] = pitch - ratio;
+ drum5_min[2] = roll - ratio;
+
+ drum5_max[0] = yaw + ratio;
+ drum5_max[1] = pitch + ratio;
+ drum5_max[2] = roll + ratio;
+ }
+ else if (j == 6){
+ drum6_min[0] = yaw - ratio;
+ drum6_min[1] = pitch - ratio;
+ drum6_min[2] = roll - ratio;
+
+ drum6_max[0] = yaw + ratio;
+ drum6_max[1] = pitch + ratio;
+ drum6_max[2] = roll + ratio;
+ }
+ else if (j == 7){
+ drum7_min[0] = yaw - ratio;
+ drum7_min[1] = pitch - ratio;
+ drum7_min[2] = roll - ratio;
+
+ drum7_max[0] = yaw + ratio;
+ drum7_max[1] = pitch + ratio;
+ drum7_max[2] = roll + ratio;
+ }
+ else if (j == 8){
+ drum8_min[0] = yaw - ratio;
+ drum8_min[1] = pitch - ratio;
+ drum8_min[2] = roll - ratio;
+
+ drum8_max[0] = yaw + ratio;
+ drum8_max[1] = pitch + ratio;
+ drum8_max[2] = roll + ratio;
+ }
+ else if (j == 9){
+ drum9_min[0] = yaw - ratio;
+ drum9_min[1] = pitch - ratio;
+ drum9_min[2] = roll - ratio;
+
+ drum9_max[0] = yaw + ratio;
+ drum9_max[1] = pitch + ratio;
+ drum9_max[2] = roll + ratio;
+ }
+ else if (j == 10){
+ drum10_min[0] = yaw - ratio;
+ drum10_min[1] = pitch - ratio;
+ drum10_min[2] = roll - ratio;
+
+ drum10_max[0] = yaw + ratio;
+ drum10_max[1] = pitch + ratio;
+ drum10_max[2] = roll + ratio;
+ }
+ if (j == 10) j = 0;
+ pc.printf("x,y,z: %f %f %f \r\n",yaw,pitch,roll);
+}
+