se
Dependencies: SDFileSystem circular_buffer MPU6050 SoftSerial
source/main.cpp@3:b2087af18efe, 2017-06-21 (annotated)
- Committer:
- OsmanKameric
- Date:
- Wed Jun 21 16:00:36 2017 +0000
- Revision:
- 3:b2087af18efe
- Parent:
- 1:2cc1c9a45be7
- Child:
- 4:147bbe6f9626
-MPU6050 update
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
OsmanKameric | 0:a4de55cab4e2 | 1 | #include <events/mbed_events.h> |
OsmanKameric | 0:a4de55cab4e2 | 2 | #include <mbed.h> |
OsmanKameric | 0:a4de55cab4e2 | 3 | #include "ble/BLE.h" |
OsmanKameric | 0:a4de55cab4e2 | 4 | #include "ble/Gap.h" |
OsmanKameric | 0:a4de55cab4e2 | 5 | #include "ble/services/BatteryService.h" |
OsmanKameric | 0:a4de55cab4e2 | 6 | #include "MPU6050.h" |
OsmanKameric | 0:a4de55cab4e2 | 7 | #include "MPUService.h" |
OsmanKameric | 1:2cc1c9a45be7 | 8 | #include <string> |
OsmanKameric | 1:2cc1c9a45be7 | 9 | #include <stdio.h> |
OsmanKameric | 3:b2087af18efe | 10 | #include "SoftSerial.h" |
OsmanKameric | 3:b2087af18efe | 11 | Serial pc(p6, p8); |
OsmanKameric | 3:b2087af18efe | 12 | //SoftSerial pc1(p9, p10); |
OsmanKameric | 0:a4de55cab4e2 | 13 | MPU6050 mpu6050; |
OsmanKameric | 3:b2087af18efe | 14 | //InterruptIn pin(p10); |
OsmanKameric | 3:b2087af18efe | 15 | //DigitalOut led(LED1, 1); |
OsmanKameric | 3:b2087af18efe | 16 | //DigitalOut led2(LED2, 1); |
OsmanKameric | 1:2cc1c9a45be7 | 17 | uint16_t customServiceUUID = 0xA000; |
OsmanKameric | 1:2cc1c9a45be7 | 18 | uint16_t readCharUUID = 0xA001; |
OsmanKameric | 3:b2087af18efe | 19 | uint16_t interruptUUID = 0xA002; |
OsmanKameric | 1:2cc1c9a45be7 | 20 | DigitalOut alivenessLED(LED1, 0); |
OsmanKameric | 0:a4de55cab4e2 | 21 | const static char DEVICE_NAME[] = "MPU"; |
OsmanKameric | 1:2cc1c9a45be7 | 22 | static const uint16_t uuid16_list[] = {0x0A0A}; |
OsmanKameric | 3:b2087af18efe | 23 | //static int counter; |
OsmanKameric | 1:2cc1c9a45be7 | 24 | /* Set Up custom Characteristics */ |
OsmanKameric | 1:2cc1c9a45be7 | 25 | static float readValue[3] = {0}; |
OsmanKameric | 3:b2087af18efe | 26 | static float interruptValue[3] = {0}; |
OsmanKameric | 1:2cc1c9a45be7 | 27 | ReadOnlyArrayGattCharacteristic<float, sizeof(readValue)> readChar(readCharUUID, readValue, GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY); |
OsmanKameric | 3:b2087af18efe | 28 | ReadOnlyArrayGattCharacteristic<float, sizeof(interruptValue)> interrupt(interruptUUID, interruptValue, GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY); |
OsmanKameric | 1:2cc1c9a45be7 | 29 | |
OsmanKameric | 1:2cc1c9a45be7 | 30 | //static float writeValue[2] = {3,4}; |
OsmanKameric | 1:2cc1c9a45be7 | 31 | //WriteOnlyArrayGattCharacteristic<float, sizeof(writeValue)> writeChar(writeCharUUID, writeValue); |
OsmanKameric | 1:2cc1c9a45be7 | 32 | |
OsmanKameric | 1:2cc1c9a45be7 | 33 | /* Set up custom service */ |
OsmanKameric | 3:b2087af18efe | 34 | GattCharacteristic *characteristics[] = {&readChar/*, &writeChar*/, &interrupt}; |
OsmanKameric | 1:2cc1c9a45be7 | 35 | GattService customService(customServiceUUID, characteristics, sizeof(characteristics) / sizeof(GattCharacteristic *)); |
OsmanKameric | 1:2cc1c9a45be7 | 36 | |
OsmanKameric | 1:2cc1c9a45be7 | 37 | |
OsmanKameric | 1:2cc1c9a45be7 | 38 | |
OsmanKameric | 0:a4de55cab4e2 | 39 | static EventQueue eventQueue(/* event count */ 10 * /* event size */ 32); |
OsmanKameric | 3:b2087af18efe | 40 | InterruptIn button(BUTTON1); |
OsmanKameric | 3:b2087af18efe | 41 | |
OsmanKameric | 3:b2087af18efe | 42 | |
OsmanKameric | 3:b2087af18efe | 43 | |
OsmanKameric | 3:b2087af18efe | 44 | |
OsmanKameric | 1:2cc1c9a45be7 | 45 | void updateReadValue() { |
OsmanKameric | 1:2cc1c9a45be7 | 46 | alivenessLED = !alivenessLED; |
OsmanKameric | 3:b2087af18efe | 47 | mpu6050.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); |
OsmanKameric | 3:b2087af18efe | 48 | |
OsmanKameric | 1:2cc1c9a45be7 | 49 | |
OsmanKameric | 1:2cc1c9a45be7 | 50 | //mpu6050.readAccelData(accelData); |
OsmanKameric | 1:2cc1c9a45be7 | 51 | //mpu6050.getAres(); |
OsmanKameric | 1:2cc1c9a45be7 | 52 | //ax = accelData[0] * aRes - accelBias[0]; |
OsmanKameric | 1:2cc1c9a45be7 | 53 | //ay = accelData[1] * aRes - accelBias[1]; |
OsmanKameric | 1:2cc1c9a45be7 | 54 | //az = accelData[2] * aRes - accelBias[2]; |
OsmanKameric | 1:2cc1c9a45be7 | 55 | float newValue[3] = {ax,ay,az}; |
OsmanKameric | 1:2cc1c9a45be7 | 56 | //mpu6050.readGyroData(gyroData); |
OsmanKameric | 1:2cc1c9a45be7 | 57 | //mpu6050.getGres(); |
OsmanKameric | 1:2cc1c9a45be7 | 58 | |
OsmanKameric | 1:2cc1c9a45be7 | 59 | //gx = gyroData[0] * gRes; // - gyroBias[0]; |
OsmanKameric | 1:2cc1c9a45be7 | 60 | //gy = gyroData[1] * gRes; // - gyroBias[1]; |
OsmanKameric | 1:2cc1c9a45be7 | 61 | //gz = gyroData[2] * gRes; // - gyroBias[2]; |
OsmanKameric | 1:2cc1c9a45be7 | 62 | //float newValue[3] = {gx,gy,gz};*/ |
OsmanKameric | 1:2cc1c9a45be7 | 63 | //counter++; |
OsmanKameric | 1:2cc1c9a45be7 | 64 | //pc.printf("| Before | ax=%.3f | ay=%.3f | az=%.3f \r\n",gx,gy,gz); |
OsmanKameric | 3:b2087af18efe | 65 | pc.printf("| Acce | ax=%.3f | ay=%.3f | az=%.3f | Gyro | gx=%.3f | gy=%.3f | gz=%.3f\r\n",ax,ay,az,gx,gy,gz); |
OsmanKameric | 1:2cc1c9a45be7 | 66 | //len_send = strlen((const char *)newValue); |
OsmanKameric | 3:b2087af18efe | 67 | |
OsmanKameric | 3:b2087af18efe | 68 | |
OsmanKameric | 3:b2087af18efe | 69 | uint8_t motionByte = mpu6050.readThisByte(MPU6050_RA_MOT_DETECT_STATUS); |
OsmanKameric | 3:b2087af18efe | 70 | pc.printf("Motion byte: %d\r\n",motionByte); |
OsmanKameric | 3:b2087af18efe | 71 | BLE::Instance(BLE::DEFAULT_INSTANCE).gattServer().write(interrupt.getValueHandle(), (uint8_t*)motionByte, 8); |
OsmanKameric | 1:2cc1c9a45be7 | 72 | BLE::Instance(BLE::DEFAULT_INSTANCE).gattServer().write(readChar.getValueHandle(), (uint8_t*)newValue, 12); |
OsmanKameric | 1:2cc1c9a45be7 | 73 | } |
OsmanKameric | 1:2cc1c9a45be7 | 74 | void periodicCallback(void) |
OsmanKameric | 1:2cc1c9a45be7 | 75 | { |
OsmanKameric | 1:2cc1c9a45be7 | 76 | if (BLE::Instance().getGapState().connected) { |
OsmanKameric | 1:2cc1c9a45be7 | 77 | eventQueue.call(updateReadValue); |
OsmanKameric | 1:2cc1c9a45be7 | 78 | } |
OsmanKameric | 1:2cc1c9a45be7 | 79 | } |
OsmanKameric | 0:a4de55cab4e2 | 80 | |
OsmanKameric | 0:a4de55cab4e2 | 81 | void scheduleBleEventsProcessing(BLE::OnEventsToProcessCallbackContext* context) |
OsmanKameric | 0:a4de55cab4e2 | 82 | { |
OsmanKameric | 0:a4de55cab4e2 | 83 | BLE& ble = BLE::Instance(); |
OsmanKameric | 0:a4de55cab4e2 | 84 | eventQueue.call(Callback<void()>(&ble, &BLE::processEvents)); |
OsmanKameric | 0:a4de55cab4e2 | 85 | } |
OsmanKameric | 0:a4de55cab4e2 | 86 | |
OsmanKameric | 0:a4de55cab4e2 | 87 | void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *params) |
OsmanKameric | 0:a4de55cab4e2 | 88 | { |
OsmanKameric | 0:a4de55cab4e2 | 89 | (void) params; |
OsmanKameric | 0:a4de55cab4e2 | 90 | BLE::Instance().gap().startAdvertising(); |
OsmanKameric | 0:a4de55cab4e2 | 91 | } |
OsmanKameric | 0:a4de55cab4e2 | 92 | void onBleInitError(BLE &ble, ble_error_t error) |
OsmanKameric | 0:a4de55cab4e2 | 93 | { |
OsmanKameric | 0:a4de55cab4e2 | 94 | /* Initialization error handling should go here */ |
OsmanKameric | 0:a4de55cab4e2 | 95 | } |
OsmanKameric | 1:2cc1c9a45be7 | 96 | |
OsmanKameric | 1:2cc1c9a45be7 | 97 | |
OsmanKameric | 0:a4de55cab4e2 | 98 | void bleInitComplete(BLE::InitializationCompleteCallbackContext* params) |
OsmanKameric | 0:a4de55cab4e2 | 99 | { |
OsmanKameric | 0:a4de55cab4e2 | 100 | BLE& ble = params->ble; |
OsmanKameric | 0:a4de55cab4e2 | 101 | ble_error_t error = params->error; |
OsmanKameric | 0:a4de55cab4e2 | 102 | |
OsmanKameric | 0:a4de55cab4e2 | 103 | if (error != BLE_ERROR_NONE) { |
OsmanKameric | 0:a4de55cab4e2 | 104 | /* In case of error, forward the error handling to onBleInitError */ |
OsmanKameric | 0:a4de55cab4e2 | 105 | onBleInitError(ble, error); |
OsmanKameric | 0:a4de55cab4e2 | 106 | return; |
OsmanKameric | 0:a4de55cab4e2 | 107 | } |
OsmanKameric | 0:a4de55cab4e2 | 108 | |
OsmanKameric | 0:a4de55cab4e2 | 109 | /* Ensure that it is the default instance of BLE */ |
OsmanKameric | 0:a4de55cab4e2 | 110 | if (ble.getInstanceID() != BLE::DEFAULT_INSTANCE) { |
OsmanKameric | 0:a4de55cab4e2 | 111 | return; |
OsmanKameric | 0:a4de55cab4e2 | 112 | } |
OsmanKameric | 0:a4de55cab4e2 | 113 | ble.gap().onDisconnection(disconnectionCallback); |
OsmanKameric | 0:a4de55cab4e2 | 114 | |
OsmanKameric | 0:a4de55cab4e2 | 115 | /* setup advertising */ |
OsmanKameric | 0:a4de55cab4e2 | 116 | ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE); |
OsmanKameric | 0:a4de55cab4e2 | 117 | ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS, (uint8_t*)uuid16_list, sizeof(uuid16_list)); |
OsmanKameric | 0:a4de55cab4e2 | 118 | ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, (uint8_t*)DEVICE_NAME, sizeof(DEVICE_NAME)); |
OsmanKameric | 0:a4de55cab4e2 | 119 | ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED); |
OsmanKameric | 1:2cc1c9a45be7 | 120 | |
OsmanKameric | 1:2cc1c9a45be7 | 121 | ble.gap().startAdvertising(); |
OsmanKameric | 1:2cc1c9a45be7 | 122 | ble.addService(customService); |
OsmanKameric | 0:a4de55cab4e2 | 123 | ble.gap().setAdvertisingInterval(1000); /* 1000ms. */ |
OsmanKameric | 0:a4de55cab4e2 | 124 | |
OsmanKameric | 0:a4de55cab4e2 | 125 | |
OsmanKameric | 0:a4de55cab4e2 | 126 | } |
OsmanKameric | 1:2cc1c9a45be7 | 127 | void flip() { |
OsmanKameric | 3:b2087af18efe | 128 | //pc.printf("Desio se interupt"); |
OsmanKameric | 1:2cc1c9a45be7 | 129 | } |
OsmanKameric | 1:2cc1c9a45be7 | 130 | bool XnegMD, XposMD, YnegMD, YposMD, ZnegMD, ZposMD; |
OsmanKameric | 0:a4de55cab4e2 | 131 | int main() |
OsmanKameric | 1:2cc1c9a45be7 | 132 | { |
OsmanKameric | 1:2cc1c9a45be7 | 133 | //mpu6050.whoAmI(); |
OsmanKameric | 1:2cc1c9a45be7 | 134 | //mpu6050.calibrate(accelBias, gyroBias); |
OsmanKameric | 1:2cc1c9a45be7 | 135 | //mpu6050.init(); |
OsmanKameric | 3:b2087af18efe | 136 | mpu6050.calibrate(accelBias, gyroBias); |
OsmanKameric | 3:b2087af18efe | 137 | mpu6050.initialize(); |
OsmanKameric | 3:b2087af18efe | 138 | mpu6050.setSleepEnabled(0); |
OsmanKameric | 3:b2087af18efe | 139 | pc.baud(115200); |
OsmanKameric | 3:b2087af18efe | 140 | mpu6050.setDHPFMode(1); |
OsmanKameric | 1:2cc1c9a45be7 | 141 | //setMotionDetectionThreshold |
OsmanKameric | 1:2cc1c9a45be7 | 142 | //char myChar = mpu6050.readByte(MPU6050_ADDRESS, 0x1F); |
OsmanKameric | 1:2cc1c9a45be7 | 143 | //pc.printf("Prije postavljanja\n"); |
OsmanKameric | 1:2cc1c9a45be7 | 144 | //pc.printf("MotionDetectionThreshold iznosi: %x\n",myChar); |
OsmanKameric | 1:2cc1c9a45be7 | 145 | //pc.printf("Poslije postavljanja\n"); |
OsmanKameric | 1:2cc1c9a45be7 | 146 | //mpu6050.writeByte(MPU6050_ADDRESS, 0x1F,0x02); |
OsmanKameric | 1:2cc1c9a45be7 | 147 | //myChar = mpu6050.readByte(MPU6050_ADDRESS, 0x1F); |
OsmanKameric | 1:2cc1c9a45be7 | 148 | //pc.printf("MotionDetectionThreshold iznosi: %x\n",myChar); |
OsmanKameric | 1:2cc1c9a45be7 | 149 | |
OsmanKameric | 1:2cc1c9a45be7 | 150 | |
OsmanKameric | 1:2cc1c9a45be7 | 151 | //setMotionDetectionDuration |
OsmanKameric | 1:2cc1c9a45be7 | 152 | //myChar = mpu6050.readByte(MPU6050_ADDRESS, 0x20); |
OsmanKameric | 1:2cc1c9a45be7 | 153 | //pc.printf("Prije postavljanja\n"); |
OsmanKameric | 1:2cc1c9a45be7 | 154 | //pc.printf("MotionDetectionDuration iznosi: %x\n",myChar); |
OsmanKameric | 1:2cc1c9a45be7 | 155 | //pc.printf("Poslije postavljanja\n"); |
OsmanKameric | 1:2cc1c9a45be7 | 156 | //mpu6050.writeByte(MPU6050_ADDRESS, 0x20,0x28); |
OsmanKameric | 1:2cc1c9a45be7 | 157 | //myChar = mpu6050.readByte(MPU6050_ADDRESS, 0x20); |
OsmanKameric | 1:2cc1c9a45be7 | 158 | //pc.printf("MotionDetectionDuration iznosi: %x\n",myChar); |
OsmanKameric | 1:2cc1c9a45be7 | 159 | |
OsmanKameric | 1:2cc1c9a45be7 | 160 | // XnegMD = mpu6050.getXNegMotionDetected(); |
OsmanKameric | 1:2cc1c9a45be7 | 161 | //mpu6050.whoAmI(); |
OsmanKameric | 1:2cc1c9a45be7 | 162 | //uint8_t *data; |
OsmanKameric | 1:2cc1c9a45be7 | 163 | //char byte = mpu6050.readByte(MPU6050_ADDRESS, 0x61); |
OsmanKameric | 1:2cc1c9a45be7 | 164 | //*data = byte & (1 << 7); |
OsmanKameric | 1:2cc1c9a45be7 | 165 | //pc.printf("Byte iznosi: %x\n",byte); |
OsmanKameric | 1:2cc1c9a45be7 | 166 | //int i = data[0]; |
OsmanKameric | 1:2cc1c9a45be7 | 167 | //pc.printf("7 bit iznosi: %d\n", i); |
OsmanKameric | 1:2cc1c9a45be7 | 168 | |
OsmanKameric | 1:2cc1c9a45be7 | 169 | //XposMD = mpu6050.getXPosMotionDetected(); |
OsmanKameric | 1:2cc1c9a45be7 | 170 | |
OsmanKameric | 1:2cc1c9a45be7 | 171 | |
OsmanKameric | 1:2cc1c9a45be7 | 172 | //pin.fall(&flip); |
OsmanKameric | 0:a4de55cab4e2 | 173 | eventQueue.call_every(500, periodicCallback); |
OsmanKameric | 0:a4de55cab4e2 | 174 | BLE& ble = BLE::Instance(); |
OsmanKameric | 0:a4de55cab4e2 | 175 | ble.onEventsToProcess(scheduleBleEventsProcessing); |
OsmanKameric | 0:a4de55cab4e2 | 176 | ble.init(bleInitComplete); |
OsmanKameric | 0:a4de55cab4e2 | 177 | eventQueue.dispatch_forever(); |
OsmanKameric | 3:b2087af18efe | 178 | while (ble.hasInitialized() == false) { /* spin loop */ } |
OsmanKameric | 3:b2087af18efe | 179 | |
OsmanKameric | 3:b2087af18efe | 180 | |
OsmanKameric | 3:b2087af18efe | 181 | |
OsmanKameric | 3:b2087af18efe | 182 | |
OsmanKameric | 3:b2087af18efe | 183 | |
OsmanKameric | 3:b2087af18efe | 184 | |
OsmanKameric | 3:b2087af18efe | 185 | // pc1.baud(9600); |
OsmanKameric | 3:b2087af18efe | 186 | //while (true) { |
OsmanKameric | 3:b2087af18efe | 187 | //ble.waitForEvent(); |
OsmanKameric | 3:b2087af18efe | 188 | // pc1.printf("tare "); |
OsmanKameric | 3:b2087af18efe | 189 | //wait_ms(1000); |
OsmanKameric | 3:b2087af18efe | 190 | |
OsmanKameric | 3:b2087af18efe | 191 | //} |
OsmanKameric | 3:b2087af18efe | 192 | //return 0; |
OsmanKameric | 0:a4de55cab4e2 | 193 | } |