Suad Suljic / Mbed OS Seismo2

se

Dependencies:   SDFileSystem circular_buffer MPU6050 SoftSerial

source/main.cpp@4:147bbe6f9626, 2017-08-23 (annotated)

Committer:
OsmanKameric
Date:
Wed Aug 23 17:26:31 2017 +0000
Revision:
4:147bbe6f9626
Parent:
3:b2087af18efe
Child:
5:ea629a3fd6c1
First

Who changed what in which revision?

UserRevisionLine numberNew 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 4:147bbe6f9626 8 #include <string>
OsmanKameric 1:2cc1c9a45be7 9 #include <stdio.h>
OsmanKameric 3:b2087af18efe 10 #include "SoftSerial.h"
OsmanKameric 4:147bbe6f9626 11 #include "circular_buffer.h"
OsmanKameric 4:147bbe6f9626 12 #include <Fsm.h>
OsmanKameric 4:147bbe6f9626 13 #include <Events.h>
OsmanKameric 4:147bbe6f9626 14 bool test=0;
OsmanKameric 4:147bbe6f9626 15 InterruptIn mpuInterrupt(p5);
OsmanKameric 4:147bbe6f9626 16 struct TurnOff turnoff;
OsmanKameric 4:147bbe6f9626 17 struct TurnOn turnon;
OsmanKameric 4:147bbe6f9626 18 struct Move move;
OsmanKameric 4:147bbe6f9626 19
OsmanKameric 4:147bbe6f9626 20 Fsm* fsm;
OsmanKameric 4:147bbe6f9626 21
OsmanKameric 4:147bbe6f9626 22 struct MotionLog {
OsmanKameric 4:147bbe6f9626 23 int motX;
OsmanKameric 4:147bbe6f9626 24 int motY;
OsmanKameric 4:147bbe6f9626 25 int motZ;
OsmanKameric 4:147bbe6f9626 26 };
OsmanKameric 4:147bbe6f9626 27 MotionLog motionLog;
OsmanKameric 4:147bbe6f9626 28 circular_buffer<MotionLog> cb(5);
OsmanKameric 4:147bbe6f9626 29
OsmanKameric 3:b2087af18efe 30 Serial pc(p6, p8);
OsmanKameric 3:b2087af18efe 31 //SoftSerial pc1(p9, p10);
OsmanKameric 0:a4de55cab4e2 32 MPU6050 mpu6050;
OsmanKameric 3:b2087af18efe 33 //InterruptIn pin(p10);
OsmanKameric 3:b2087af18efe 34 //DigitalOut led(LED1, 1);
OsmanKameric 3:b2087af18efe 35 //DigitalOut led2(LED2, 1);
OsmanKameric 1:2cc1c9a45be7 36 uint16_t customServiceUUID = 0xA000;
OsmanKameric 1:2cc1c9a45be7 37 uint16_t readCharUUID = 0xA001;
OsmanKameric 3:b2087af18efe 38 uint16_t interruptUUID = 0xA002;
OsmanKameric 4:147bbe6f9626 39 uint16_t gyroUUID = 0xA003;
OsmanKameric 4:147bbe6f9626 40
OsmanKameric 4:147bbe6f9626 41
OsmanKameric 4:147bbe6f9626 42
OsmanKameric 4:147bbe6f9626 43 uint16_t motionThresholdUUID = 0xA004;
OsmanKameric 4:147bbe6f9626 44 static uint8_t motionThresholdValue[10] = {0};
OsmanKameric 4:147bbe6f9626 45 WriteOnlyArrayGattCharacteristic<uint8_t, sizeof(motionThresholdValue)> motionThreshold(motionThresholdUUID, motionThresholdValue);
OsmanKameric 4:147bbe6f9626 46
OsmanKameric 4:147bbe6f9626 47
OsmanKameric 4:147bbe6f9626 48 uint16_t motionDurationUUID = 0xA005;
OsmanKameric 4:147bbe6f9626 49 static uint8_t motionDurationValue[10] = {0};
OsmanKameric 4:147bbe6f9626 50 WriteOnlyArrayGattCharacteristic<uint8_t, sizeof(motionDurationValue)> motionDuration(motionDurationUUID, motionDurationValue);
OsmanKameric 4:147bbe6f9626 51
OsmanKameric 4:147bbe6f9626 52
OsmanKameric 4:147bbe6f9626 53 uint16_t zeroMotionThresholdUUID = 0xA006;
OsmanKameric 4:147bbe6f9626 54 static uint8_t zeroMotionThresholdValue[10] = {0};
OsmanKameric 4:147bbe6f9626 55 WriteOnlyArrayGattCharacteristic<uint8_t, sizeof(zeroMotionThresholdValue)> zeroMotionThreshold(zeroMotionThresholdUUID, zeroMotionThresholdValue);
OsmanKameric 4:147bbe6f9626 56
OsmanKameric 4:147bbe6f9626 57
OsmanKameric 4:147bbe6f9626 58 uint16_t zeroMotionDurationUUID = 0xA007;
OsmanKameric 4:147bbe6f9626 59 static uint8_t zeroMotionDurationValue[10] = {0};
OsmanKameric 4:147bbe6f9626 60 WriteOnlyArrayGattCharacteristic<uint8_t, sizeof(zeroMotionDurationValue)> zeroMotionDuration(zeroMotionDurationUUID, zeroMotionDurationValue);
OsmanKameric 4:147bbe6f9626 61
OsmanKameric 4:147bbe6f9626 62 uint16_t fsmStateUUID = 0xA008;
OsmanKameric 4:147bbe6f9626 63 static float fsmStateValue[3] = {0};
OsmanKameric 4:147bbe6f9626 64 ReadOnlyArrayGattCharacteristic<float, sizeof(fsmStateValue)> fsmState(fsmStateUUID, fsmStateValue, GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY);
OsmanKameric 4:147bbe6f9626 65
OsmanKameric 1:2cc1c9a45be7 66 DigitalOut alivenessLED(LED1, 0);
OsmanKameric 0:a4de55cab4e2 67 const static char DEVICE_NAME[] = "MPU";
OsmanKameric 1:2cc1c9a45be7 68 static const uint16_t uuid16_list[] = {0x0A0A};
OsmanKameric 3:b2087af18efe 69 //static int counter;
OsmanKameric 1:2cc1c9a45be7 70 /* Set Up custom Characteristics */
OsmanKameric 1:2cc1c9a45be7 71 static float readValue[3] = {0};
OsmanKameric 3:b2087af18efe 72 static float interruptValue[3] = {0};
OsmanKameric 4:147bbe6f9626 73 static float gyroValue[3] = {0};
OsmanKameric 4:147bbe6f9626 74
OsmanKameric 4:147bbe6f9626 75
OsmanKameric 1:2cc1c9a45be7 76 ReadOnlyArrayGattCharacteristic<float, sizeof(readValue)> readChar(readCharUUID, readValue, GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY);
OsmanKameric 3:b2087af18efe 77 ReadOnlyArrayGattCharacteristic<float, sizeof(interruptValue)> interrupt(interruptUUID, interruptValue, GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY);
OsmanKameric 4:147bbe6f9626 78 ReadOnlyArrayGattCharacteristic<float, sizeof(gyroValue)> gyro(gyroUUID, gyroValue, GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY);
OsmanKameric 1:2cc1c9a45be7 79
OsmanKameric 1:2cc1c9a45be7 80 //static float writeValue[2] = {3,4};
OsmanKameric 1:2cc1c9a45be7 81 //WriteOnlyArrayGattCharacteristic<float, sizeof(writeValue)> writeChar(writeCharUUID, writeValue);
OsmanKameric 1:2cc1c9a45be7 82
OsmanKameric 1:2cc1c9a45be7 83 /* Set up custom service */
OsmanKameric 4:147bbe6f9626 84 GattCharacteristic *characteristics[] = {&readChar/*, &writeChar*/, &interrupt, &gyro, &motionThreshold, &motionDuration, &zeroMotionThreshold, &zeroMotionDuration, &fsmState};
OsmanKameric 1:2cc1c9a45be7 85 GattService customService(customServiceUUID, characteristics, sizeof(characteristics) / sizeof(GattCharacteristic *));
OsmanKameric 1:2cc1c9a45be7 86
OsmanKameric 1:2cc1c9a45be7 87
OsmanKameric 1:2cc1c9a45be7 88
OsmanKameric 0:a4de55cab4e2 89 static EventQueue eventQueue(/* event count */ 10 * /* event size */ 32);
OsmanKameric 3:b2087af18efe 90 InterruptIn button(BUTTON1);
OsmanKameric 3:b2087af18efe 91
OsmanKameric 3:b2087af18efe 92
OsmanKameric 3:b2087af18efe 93
OsmanKameric 3:b2087af18efe 94
OsmanKameric 4:147bbe6f9626 95 void updateReadValue()
OsmanKameric 4:147bbe6f9626 96 {
OsmanKameric 4:147bbe6f9626 97 alivenessLED = !alivenessLED;
OsmanKameric 4:147bbe6f9626 98 //mpu6050.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
OsmanKameric 4:147bbe6f9626 99 //mpu6050.getAcceleration(&ax, &ay, &az);
OsmanKameric 4:147bbe6f9626 100
OsmanKameric 4:147bbe6f9626 101 mpu6050.readAccelData(accelData);
OsmanKameric 4:147bbe6f9626 102 mpu6050.getAres();
OsmanKameric 4:147bbe6f9626 103 ax = accelData[0]*aRes - accelBias[0];
OsmanKameric 4:147bbe6f9626 104 ay = accelData[1]*aRes - accelBias[1];
OsmanKameric 4:147bbe6f9626 105 az = accelData[2]*aRes - accelBias[2];
OsmanKameric 4:147bbe6f9626 106 float accelValues[3] = {ax,ay,az};
OsmanKameric 4:147bbe6f9626 107
OsmanKameric 4:147bbe6f9626 108 mpu6050.readGyroData(gyroData);
OsmanKameric 4:147bbe6f9626 109 mpu6050.getGres();
OsmanKameric 4:147bbe6f9626 110 gx = gyroData[0]*gRes - gyroBias[0];
OsmanKameric 4:147bbe6f9626 111 gy = gyroData[1]*gRes - gyroBias[1];
OsmanKameric 4:147bbe6f9626 112 gz = gyroData[2]*gRes - gyroBias[2];
OsmanKameric 4:147bbe6f9626 113 float gyroValues[3] = {gx,gy,gz};
OsmanKameric 4:147bbe6f9626 114
OsmanKameric 1:2cc1c9a45be7 115 //counter++;
OsmanKameric 1:2cc1c9a45be7 116 //pc.printf("| Before | ax=%.3f | ay=%.3f | az=%.3f \r\n",gx,gy,gz);
OsmanKameric 4:147bbe6f9626 117 //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 118 //len_send = strlen((const char *)newValue);
OsmanKameric 4:147bbe6f9626 119
OsmanKameric 3:b2087af18efe 120
OsmanKameric 3:b2087af18efe 121 uint8_t motionByte = mpu6050.readThisByte(MPU6050_RA_MOT_DETECT_STATUS);
OsmanKameric 4:147bbe6f9626 122 uint8_t b;
OsmanKameric 4:147bbe6f9626 123 bool zero = false;
OsmanKameric 4:147bbe6f9626 124 float fx, fy,fz=0;
OsmanKameric 4:147bbe6f9626 125 //char x,y,z;
OsmanKameric 4:147bbe6f9626 126 int mx=0;
OsmanKameric 4:147bbe6f9626 127 int my=0;
OsmanKameric 4:147bbe6f9626 128 int mz=0;
OsmanKameric 4:147bbe6f9626 129 //x=y=z='0';
OsmanKameric 4:147bbe6f9626 130 for(int i = 0; i<=7; i++) {
OsmanKameric 4:147bbe6f9626 131 b = motionByte & (1<<i);
OsmanKameric 4:147bbe6f9626 132 if(i==0 && b==1) {
OsmanKameric 4:147bbe6f9626 133 zero = true;
OsmanKameric 4:147bbe6f9626 134 //pc.printf(" %c %c %c | ",x,y,z);
OsmanKameric 4:147bbe6f9626 135 //pc.printf(" | %.3f | %.3f | %.3f | %.3f | %.3f | %.3f ZERO DETECTED\r\n",ax,ay,az, gx, gy, gz);
OsmanKameric 4:147bbe6f9626 136 //pc.printf(" | %d | %d | %d | %d | %d | %d ZERO DETECTED\r\n",ax,ay,az, gx, gy, gz);
OsmanKameric 4:147bbe6f9626 137 }
OsmanKameric 4:147bbe6f9626 138 if(i==1 && b==2) {
OsmanKameric 4:147bbe6f9626 139 //pc.printf("This should not happen :).\r\n");
OsmanKameric 4:147bbe6f9626 140 }
OsmanKameric 4:147bbe6f9626 141 if(i==6 && b==64) {
OsmanKameric 4:147bbe6f9626 142 //x='+';
OsmanKameric 4:147bbe6f9626 143 fx=1;
OsmanKameric 4:147bbe6f9626 144 mx=1;
OsmanKameric 4:147bbe6f9626 145 //pc.printf("X POSITIVE motion detected.\r\n");
OsmanKameric 4:147bbe6f9626 146 }
OsmanKameric 4:147bbe6f9626 147 if(i==7 && b==128) {
OsmanKameric 4:147bbe6f9626 148 //x='-';
OsmanKameric 4:147bbe6f9626 149 fx=2;
OsmanKameric 4:147bbe6f9626 150 mx=1;
OsmanKameric 4:147bbe6f9626 151 //pc.printf("X NEGATIVE motion detected.\r\n");
OsmanKameric 4:147bbe6f9626 152 }
OsmanKameric 4:147bbe6f9626 153 if(i==4 && b==16) {
OsmanKameric 4:147bbe6f9626 154 //y='+';
OsmanKameric 4:147bbe6f9626 155 fy=1;
OsmanKameric 4:147bbe6f9626 156 my=1;
OsmanKameric 4:147bbe6f9626 157 //pc.printf("Y POSITIVE motion detected.\r\n");
OsmanKameric 4:147bbe6f9626 158 }
OsmanKameric 4:147bbe6f9626 159 if(i==5 && b==32) {
OsmanKameric 4:147bbe6f9626 160 //y='-';
OsmanKameric 4:147bbe6f9626 161 fy=2;
OsmanKameric 4:147bbe6f9626 162 my=1;
OsmanKameric 4:147bbe6f9626 163 //pc.printf("Y NEGATIVE motion detected.\r\n");
OsmanKameric 4:147bbe6f9626 164 }
OsmanKameric 4:147bbe6f9626 165 if(i==2 && b==4) {
OsmanKameric 4:147bbe6f9626 166 //z='+';
OsmanKameric 4:147bbe6f9626 167 fz=1;
OsmanKameric 4:147bbe6f9626 168 mz=1;
OsmanKameric 4:147bbe6f9626 169 //pc.printf("Z POSITIVE motion detected.\r\n");
OsmanKameric 4:147bbe6f9626 170 }
OsmanKameric 4:147bbe6f9626 171 if(i==3 && b==8) {
OsmanKameric 4:147bbe6f9626 172 //z='-';
OsmanKameric 4:147bbe6f9626 173 fz=2;
OsmanKameric 4:147bbe6f9626 174 mz=1;
OsmanKameric 4:147bbe6f9626 175 //pc.printf("Z NEGATIVE motion detected.\r\n");
OsmanKameric 4:147bbe6f9626 176 }
OsmanKameric 4:147bbe6f9626 177 }
OsmanKameric 4:147bbe6f9626 178 // if(test) {
OsmanKameric 4:147bbe6f9626 179 motionLog.motX = mx;
OsmanKameric 4:147bbe6f9626 180 motionLog.motY = my;
OsmanKameric 4:147bbe6f9626 181 motionLog.motZ = mz;
OsmanKameric 4:147bbe6f9626 182
OsmanKameric 4:147bbe6f9626 183 cb.push_back(motionLog);
OsmanKameric 4:147bbe6f9626 184 //pc.printf("Desio se motion\r\n");
OsmanKameric 4:147bbe6f9626 185 // test=0;
OsmanKameric 4:147bbe6f9626 186 //}
OsmanKameric 4:147bbe6f9626 187
OsmanKameric 4:147bbe6f9626 188 int numX, numY, numZ=0;
OsmanKameric 4:147bbe6f9626 189 //pc.printf("\r\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n");
OsmanKameric 4:147bbe6f9626 190 numX=numY=numZ=0;
OsmanKameric 4:147bbe6f9626 191 for(int i=0; i<=4; i++) {
OsmanKameric 4:147bbe6f9626 192 //pc.printf("%d %d %d\r\n",cb[i].motX, cb[i].motY, cb[i].motZ);
OsmanKameric 4:147bbe6f9626 193 if(cb[i].motX!=0) {
OsmanKameric 4:147bbe6f9626 194 numX++;
OsmanKameric 4:147bbe6f9626 195 }
OsmanKameric 4:147bbe6f9626 196 if(cb[i].motY!=0) {
OsmanKameric 4:147bbe6f9626 197 numY++;
OsmanKameric 4:147bbe6f9626 198 }
OsmanKameric 4:147bbe6f9626 199 if(cb[i].motZ!=0) {
OsmanKameric 4:147bbe6f9626 200 numZ++;
OsmanKameric 4:147bbe6f9626 201 }
OsmanKameric 4:147bbe6f9626 202 }
OsmanKameric 4:147bbe6f9626 203 if(numX<=2) {
OsmanKameric 4:147bbe6f9626 204 numX=0;
OsmanKameric 4:147bbe6f9626 205 } else
OsmanKameric 4:147bbe6f9626 206 numX=1;
OsmanKameric 4:147bbe6f9626 207
OsmanKameric 4:147bbe6f9626 208 if(numY<=2) {
OsmanKameric 4:147bbe6f9626 209 numY=0;
OsmanKameric 4:147bbe6f9626 210 } else
OsmanKameric 4:147bbe6f9626 211 numY=1;
OsmanKameric 4:147bbe6f9626 212
OsmanKameric 4:147bbe6f9626 213 if(numZ<=2) {
OsmanKameric 4:147bbe6f9626 214 numZ=0;
OsmanKameric 4:147bbe6f9626 215 } else
OsmanKameric 4:147bbe6f9626 216 numZ=1;
OsmanKameric 4:147bbe6f9626 217
OsmanKameric 4:147bbe6f9626 218 if(numX!=0 || numY!=0 || numZ!=0){
OsmanKameric 4:147bbe6f9626 219 numX=1;
OsmanKameric 4:147bbe6f9626 220 numY=1;
OsmanKameric 4:147bbe6f9626 221 numZ=1;
OsmanKameric 4:147bbe6f9626 222 }
OsmanKameric 4:147bbe6f9626 223
OsmanKameric 4:147bbe6f9626 224
OsmanKameric 4:147bbe6f9626 225 //pc.printf("***************************\r\n");
OsmanKameric 4:147bbe6f9626 226 //pc.printf("%d %d %d\r\n",numX,numY,numZ);
OsmanKameric 4:147bbe6f9626 227 float fsmStateValues[3] = {numX,numY,numZ};
OsmanKameric 4:147bbe6f9626 228 if(!zero) {
OsmanKameric 4:147bbe6f9626 229 //pc.printf(" %c %c %c | ",x,y,z);
OsmanKameric 4:147bbe6f9626 230 //pc.printf(" | %.3f | %.3f | %.3f | %.3f | %.3f | %.3f\r\n",ax,ay,az, gx, gy, gz);
OsmanKameric 4:147bbe6f9626 231 //pc.printf(" | %d | %d | %d | %d | %d | %d\r\n",ax,ay,az, gx, gy, gz);
OsmanKameric 4:147bbe6f9626 232 }
OsmanKameric 4:147bbe6f9626 233
OsmanKameric 4:147bbe6f9626 234 float motionValues[3] = {fx,fy,fz};
OsmanKameric 4:147bbe6f9626 235
OsmanKameric 4:147bbe6f9626 236 BLE::Instance(BLE::DEFAULT_INSTANCE).gattServer().write(interrupt.getValueHandle(), (uint8_t*)motionValues, 12);
OsmanKameric 4:147bbe6f9626 237 BLE::Instance(BLE::DEFAULT_INSTANCE).gattServer().write(readChar.getValueHandle(), (uint8_t*)accelValues, 12);
OsmanKameric 4:147bbe6f9626 238 BLE::Instance(BLE::DEFAULT_INSTANCE).gattServer().write(gyro.getValueHandle(), (uint8_t*)gyroValues, 12);
OsmanKameric 4:147bbe6f9626 239 BLE::Instance(BLE::DEFAULT_INSTANCE).gattServer().write(fsmState.getValueHandle(), (uint8_t*)fsmStateValues, 12);
OsmanKameric 1:2cc1c9a45be7 240 }
OsmanKameric 1:2cc1c9a45be7 241 void periodicCallback(void)
OsmanKameric 1:2cc1c9a45be7 242 {
OsmanKameric 4:147bbe6f9626 243 //if (BLE::Instance().getGapState().connected) {
OsmanKameric 4:147bbe6f9626 244 // eventQueue.call(updateReadValue);
OsmanKameric 4:147bbe6f9626 245 //}
OsmanKameric 1:2cc1c9a45be7 246 }
OsmanKameric 0:a4de55cab4e2 247
OsmanKameric 0:a4de55cab4e2 248 void scheduleBleEventsProcessing(BLE::OnEventsToProcessCallbackContext* context)
OsmanKameric 0:a4de55cab4e2 249 {
OsmanKameric 0:a4de55cab4e2 250 BLE& ble = BLE::Instance();
OsmanKameric 0:a4de55cab4e2 251 eventQueue.call(Callback<void()>(&ble, &BLE::processEvents));
OsmanKameric 0:a4de55cab4e2 252 }
OsmanKameric 0:a4de55cab4e2 253
OsmanKameric 0:a4de55cab4e2 254 void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *params)
OsmanKameric 0:a4de55cab4e2 255 {
OsmanKameric 0:a4de55cab4e2 256 (void) params;
OsmanKameric 0:a4de55cab4e2 257 BLE::Instance().gap().startAdvertising();
OsmanKameric 0:a4de55cab4e2 258 }
OsmanKameric 0:a4de55cab4e2 259 void onBleInitError(BLE &ble, ble_error_t error)
OsmanKameric 0:a4de55cab4e2 260 {
OsmanKameric 0:a4de55cab4e2 261 /* Initialization error handling should go here */
OsmanKameric 0:a4de55cab4e2 262 }
OsmanKameric 4:147bbe6f9626 263 void writeCharCallback(const GattWriteCallbackParams *params)
OsmanKameric 4:147bbe6f9626 264 {
OsmanKameric 4:147bbe6f9626 265 printf("Nesto ima na ulazu\r\n");
OsmanKameric 4:147bbe6f9626 266 printf("Handle je: %d\r\n", params->handle);
OsmanKameric 4:147bbe6f9626 267 if(params->handle == motionThreshold.getValueHandle()) {
OsmanKameric 4:147bbe6f9626 268 uint8_t value = params->data[0];
OsmanKameric 4:147bbe6f9626 269 printf("Value is: %d\r\n", value);
OsmanKameric 4:147bbe6f9626 270 mpu6050.setMotionDetectionThreshold(value);
OsmanKameric 4:147bbe6f9626 271 uint8_t motionDetectionThreshold = mpu6050.getMotionDetectionThreshold();
OsmanKameric 4:147bbe6f9626 272 pc.printf("MPU6050 motion detection threshold is: %d \r\n", motionDetectionThreshold);
OsmanKameric 1:2cc1c9a45be7 273
OsmanKameric 4:147bbe6f9626 274 //printf("Data received: length = %d, data = 0x",params->len);
OsmanKameric 4:147bbe6f9626 275
OsmanKameric 4:147bbe6f9626 276
OsmanKameric 4:147bbe6f9626 277 // for(int x=0; x < params->len; x++) {
OsmanKameric 4:147bbe6f9626 278 // printf("%x", params->data[x]);
OsmanKameric 4:147bbe6f9626 279 // }
OsmanKameric 4:147bbe6f9626 280 // printf("\n\r");
OsmanKameric 4:147bbe6f9626 281 }
OsmanKameric 4:147bbe6f9626 282
OsmanKameric 4:147bbe6f9626 283 if(params->handle == motionDuration.getValueHandle()) {
OsmanKameric 4:147bbe6f9626 284 uint8_t value = params->data[0];
OsmanKameric 4:147bbe6f9626 285 printf("Value is: %d\r\n", value);
OsmanKameric 4:147bbe6f9626 286 mpu6050.setMotionDetectionDuration(value);
OsmanKameric 4:147bbe6f9626 287 uint8_t motionDetectionDuration = mpu6050.getMotionDetectionDuration();
OsmanKameric 4:147bbe6f9626 288 pc.printf("MPU6050 motion detection duration is: %d \r\n", motionDetectionDuration);
OsmanKameric 4:147bbe6f9626 289 //printf("Data received: length = %d, data = 0x",params->len);
OsmanKameric 4:147bbe6f9626 290
OsmanKameric 4:147bbe6f9626 291
OsmanKameric 4:147bbe6f9626 292 // for(int x=0; x < params->len; x++) {
OsmanKameric 4:147bbe6f9626 293 // printf("%x", params->data[x]);
OsmanKameric 4:147bbe6f9626 294 // }
OsmanKameric 4:147bbe6f9626 295 // printf("\n\r");
OsmanKameric 4:147bbe6f9626 296 }
OsmanKameric 4:147bbe6f9626 297 if(params->handle == zeroMotionThreshold.getValueHandle()) {
OsmanKameric 4:147bbe6f9626 298 uint8_t value = params->data[0];
OsmanKameric 4:147bbe6f9626 299 printf("Value is: %d\r\n", value);
OsmanKameric 4:147bbe6f9626 300 mpu6050.setZeroMotionDetectionThreshold(value);
OsmanKameric 4:147bbe6f9626 301 uint8_t zeroMotionDetectionThreshold = mpu6050.getZeroMotionDetectionThreshold();
OsmanKameric 4:147bbe6f9626 302 pc.printf("MPU6050 zero motion detection threshold is: %d \r\n", zeroMotionDetectionThreshold);
OsmanKameric 4:147bbe6f9626 303
OsmanKameric 4:147bbe6f9626 304 }
OsmanKameric 4:147bbe6f9626 305 if(params->handle == zeroMotionDuration.getValueHandle()) {
OsmanKameric 4:147bbe6f9626 306 uint8_t value = params->data[0];
OsmanKameric 4:147bbe6f9626 307 printf("Value is: %d\r\n", value);
OsmanKameric 4:147bbe6f9626 308 mpu6050.setZeroMotionDetectionDuration(value);
OsmanKameric 4:147bbe6f9626 309 uint8_t zeroMotionDetectionDuration = mpu6050.getZeroMotionDetectionDuration();
OsmanKameric 4:147bbe6f9626 310 pc.printf("MPU6050 zero motion detection duration is: %d \r\n", zeroMotionDetectionDuration);
OsmanKameric 4:147bbe6f9626 311
OsmanKameric 4:147bbe6f9626 312 }
OsmanKameric 4:147bbe6f9626 313 }
OsmanKameric 1:2cc1c9a45be7 314
OsmanKameric 0:a4de55cab4e2 315 void bleInitComplete(BLE::InitializationCompleteCallbackContext* params)
OsmanKameric 0:a4de55cab4e2 316 {
OsmanKameric 0:a4de55cab4e2 317 BLE& ble = params->ble;
OsmanKameric 0:a4de55cab4e2 318 ble_error_t error = params->error;
OsmanKameric 0:a4de55cab4e2 319
OsmanKameric 0:a4de55cab4e2 320 if (error != BLE_ERROR_NONE) {
OsmanKameric 0:a4de55cab4e2 321 /* In case of error, forward the error handling to onBleInitError */
OsmanKameric 0:a4de55cab4e2 322 onBleInitError(ble, error);
OsmanKameric 0:a4de55cab4e2 323 return;
OsmanKameric 0:a4de55cab4e2 324 }
OsmanKameric 0:a4de55cab4e2 325
OsmanKameric 0:a4de55cab4e2 326 /* Ensure that it is the default instance of BLE */
OsmanKameric 0:a4de55cab4e2 327 if (ble.getInstanceID() != BLE::DEFAULT_INSTANCE) {
OsmanKameric 0:a4de55cab4e2 328 return;
OsmanKameric 0:a4de55cab4e2 329 }
OsmanKameric 0:a4de55cab4e2 330 ble.gap().onDisconnection(disconnectionCallback);
OsmanKameric 4:147bbe6f9626 331 ble.gattServer().onDataWritten(writeCharCallback);
OsmanKameric 0:a4de55cab4e2 332 /* setup advertising */
OsmanKameric 0:a4de55cab4e2 333 ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE);
OsmanKameric 0:a4de55cab4e2 334 ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS, (uint8_t*)uuid16_list, sizeof(uuid16_list));
OsmanKameric 0:a4de55cab4e2 335 ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, (uint8_t*)DEVICE_NAME, sizeof(DEVICE_NAME));
OsmanKameric 0:a4de55cab4e2 336 ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
OsmanKameric 4:147bbe6f9626 337
OsmanKameric 1:2cc1c9a45be7 338 ble.gap().startAdvertising();
OsmanKameric 1:2cc1c9a45be7 339 ble.addService(customService);
OsmanKameric 0:a4de55cab4e2 340 ble.gap().setAdvertisingInterval(1000); /* 1000ms. */
OsmanKameric 4:147bbe6f9626 341
OsmanKameric 0:a4de55cab4e2 342
OsmanKameric 0:a4de55cab4e2 343 }
OsmanKameric 4:147bbe6f9626 344 void flip()
OsmanKameric 4:147bbe6f9626 345 {
OsmanKameric 3:b2087af18efe 346 //pc.printf("Desio se interupt");
OsmanKameric 1:2cc1c9a45be7 347 }
OsmanKameric 4:147bbe6f9626 348 void mpuInterruptCallback()
OsmanKameric 4:147bbe6f9626 349 {
OsmanKameric 4:147bbe6f9626 350 //test=1;
OsmanKameric 4:147bbe6f9626 351
OsmanKameric 4:147bbe6f9626 352 if (BLE::Instance().getGapState().connected) {
OsmanKameric 4:147bbe6f9626 353 eventQueue.call(updateReadValue);
OsmanKameric 4:147bbe6f9626 354 }
OsmanKameric 4:147bbe6f9626 355
OsmanKameric 4:147bbe6f9626 356 }
OsmanKameric 1:2cc1c9a45be7 357 bool XnegMD, XposMD, YnegMD, YposMD, ZnegMD, ZposMD;
OsmanKameric 0:a4de55cab4e2 358 int main()
OsmanKameric 4:147bbe6f9626 359 {
OsmanKameric 1:2cc1c9a45be7 360 //mpu6050.whoAmI();
OsmanKameric 1:2cc1c9a45be7 361 //mpu6050.calibrate(accelBias, gyroBias);
OsmanKameric 1:2cc1c9a45be7 362 //mpu6050.init();
OsmanKameric 4:147bbe6f9626 363
OsmanKameric 4:147bbe6f9626 364 motionLog.motX=0;
OsmanKameric 4:147bbe6f9626 365 motionLog.motY=0;
OsmanKameric 4:147bbe6f9626 366 motionLog.motZ=0;
OsmanKameric 4:147bbe6f9626 367 for(int i=0; i<=4; i++) {
OsmanKameric 4:147bbe6f9626 368 cb.push_back(motionLog);
OsmanKameric 4:147bbe6f9626 369 }
OsmanKameric 4:147bbe6f9626 370
OsmanKameric 4:147bbe6f9626 371
OsmanKameric 3:b2087af18efe 372 mpu6050.calibrate(accelBias, gyroBias);
OsmanKameric 3:b2087af18efe 373 mpu6050.initialize();
OsmanKameric 3:b2087af18efe 374 mpu6050.setSleepEnabled(0);
OsmanKameric 3:b2087af18efe 375 pc.baud(115200);
OsmanKameric 4:147bbe6f9626 376 mpuInterrupt.fall(mpuInterruptCallback);
OsmanKameric 4:147bbe6f9626 377
OsmanKameric 4:147bbe6f9626 378 //mpu6050.setIntEnabled(1);
OsmanKameric 4:147bbe6f9626 379 mpu6050.setIntZeroMotionEnabled(1);
OsmanKameric 4:147bbe6f9626 380 mpu6050.setIntMotionEnabled(1);
OsmanKameric 3:b2087af18efe 381 mpu6050.setDHPFMode(1);
OsmanKameric 4:147bbe6f9626 382
OsmanKameric 4:147bbe6f9626 383 pc.printf("\n\n\n");
OsmanKameric 4:147bbe6f9626 384 uint8_t motionDetectionThreshold = mpu6050.getMotionDetectionThreshold();
OsmanKameric 4:147bbe6f9626 385 //pc.printf("MPU6050 motion detection threshold is: %d \r\n", motionDetectionThreshold);
OsmanKameric 4:147bbe6f9626 386 //pc.printf("MPU6050 set motion detection threshold to 2\r\n");
OsmanKameric 4:147bbe6f9626 387 mpu6050.setMotionDetectionThreshold(1);
OsmanKameric 4:147bbe6f9626 388 motionDetectionThreshold = mpu6050.getMotionDetectionThreshold();
OsmanKameric 4:147bbe6f9626 389 //pc.printf("MPU6050 motion detection threshold now is: %d \r\n", motionDetectionThreshold);
OsmanKameric 4:147bbe6f9626 390 pc.printf("Motion detection threshold is: %d \r\n", motionDetectionThreshold);
OsmanKameric 4:147bbe6f9626 391 uint8_t motionDetectionDuration = mpu6050.getMotionDetectionDuration();
OsmanKameric 4:147bbe6f9626 392 //pc.printf("MPU6050 motion detection duration is: %d \r\n", motionDetectionDuration);
OsmanKameric 4:147bbe6f9626 393 //pc.printf("MPU6050 set motion detection duration to 40\r\n");
OsmanKameric 4:147bbe6f9626 394 mpu6050.setMotionDetectionDuration(20);
OsmanKameric 4:147bbe6f9626 395 motionDetectionDuration = mpu6050.getMotionDetectionDuration();
OsmanKameric 4:147bbe6f9626 396 //pc.printf("MPU6050 motion detection duration now is: %d \r\n", motionDetectionDuration);
OsmanKameric 4:147bbe6f9626 397 pc.printf("Motion detection duration is: %d \r\n", motionDetectionDuration);
OsmanKameric 4:147bbe6f9626 398
OsmanKameric 4:147bbe6f9626 399 //*******************************************************************************************************************************
OsmanKameric 4:147bbe6f9626 400
OsmanKameric 4:147bbe6f9626 401 //pc.printf("\n\n\n");
OsmanKameric 4:147bbe6f9626 402 uint8_t zeroMotionDetectionThreshold = mpu6050.getZeroMotionDetectionThreshold();
OsmanKameric 4:147bbe6f9626 403 //pc.printf("MPU6050 zero motion detection threshold is: %d \r\n", zeroMotionDetectionThreshold);
OsmanKameric 4:147bbe6f9626 404 //pc.printf("MPU6050 set zero motion detection threshold to 2\r\n");
OsmanKameric 4:147bbe6f9626 405 mpu6050.setZeroMotionDetectionThreshold(2);
OsmanKameric 4:147bbe6f9626 406 zeroMotionDetectionThreshold = mpu6050.getZeroMotionDetectionThreshold();
OsmanKameric 4:147bbe6f9626 407 //pc.printf("MPU6050 zero motion detection threshold now is: %d \r\n", zeroMotionDetectionThreshold);
OsmanKameric 4:147bbe6f9626 408 pc.printf("Zero motion detection threshold is: %d \r\n", zeroMotionDetectionThreshold);
OsmanKameric 4:147bbe6f9626 409 //pc.printf("\n\n\n");
OsmanKameric 4:147bbe6f9626 410 int8_t zeroMotionDetectionDuration = mpu6050.getZeroMotionDetectionDuration();
OsmanKameric 4:147bbe6f9626 411 //pc.printf("MPU6050 zero motion detection duration is: %d \r\n", zeroMotionDetectionDuration);
OsmanKameric 4:147bbe6f9626 412 //pc.printf("MPU6050 set zero motion detection duration to 1\r\n");
OsmanKameric 4:147bbe6f9626 413 mpu6050.setZeroMotionDetectionDuration(1);
OsmanKameric 4:147bbe6f9626 414 zeroMotionDetectionDuration = mpu6050.getZeroMotionDetectionDuration();
OsmanKameric 4:147bbe6f9626 415 //pc.printf("MPU6050 zero motion detection duration now is: %d \r\n", zeroMotionDetectionDuration);
OsmanKameric 4:147bbe6f9626 416 pc.printf("Zero motion detection duration is: %d \r\n", zeroMotionDetectionDuration);
OsmanKameric 4:147bbe6f9626 417
OsmanKameric 4:147bbe6f9626 418 //*******************************************************************************************************************************
OsmanKameric 1:2cc1c9a45be7 419 //setMotionDetectionThreshold
OsmanKameric 1:2cc1c9a45be7 420 //char myChar = mpu6050.readByte(MPU6050_ADDRESS, 0x1F);
OsmanKameric 1:2cc1c9a45be7 421 //pc.printf("Prije postavljanja\n");
OsmanKameric 1:2cc1c9a45be7 422 //pc.printf("MotionDetectionThreshold iznosi: %x\n",myChar);
OsmanKameric 4:147bbe6f9626 423 //pc.printf("Poslije postavljanja\n");
OsmanKameric 1:2cc1c9a45be7 424 //mpu6050.writeByte(MPU6050_ADDRESS, 0x1F,0x02);
OsmanKameric 1:2cc1c9a45be7 425 //myChar = mpu6050.readByte(MPU6050_ADDRESS, 0x1F);
OsmanKameric 1:2cc1c9a45be7 426 //pc.printf("MotionDetectionThreshold iznosi: %x\n",myChar);
OsmanKameric 4:147bbe6f9626 427
OsmanKameric 4:147bbe6f9626 428
OsmanKameric 1:2cc1c9a45be7 429 //setMotionDetectionDuration
OsmanKameric 1:2cc1c9a45be7 430 //myChar = mpu6050.readByte(MPU6050_ADDRESS, 0x20);
OsmanKameric 1:2cc1c9a45be7 431 //pc.printf("Prije postavljanja\n");
OsmanKameric 1:2cc1c9a45be7 432 //pc.printf("MotionDetectionDuration iznosi: %x\n",myChar);
OsmanKameric 4:147bbe6f9626 433 //pc.printf("Poslije postavljanja\n");
OsmanKameric 1:2cc1c9a45be7 434 //mpu6050.writeByte(MPU6050_ADDRESS, 0x20,0x28);
OsmanKameric 1:2cc1c9a45be7 435 //myChar = mpu6050.readByte(MPU6050_ADDRESS, 0x20);
OsmanKameric 1:2cc1c9a45be7 436 //pc.printf("MotionDetectionDuration iznosi: %x\n",myChar);
OsmanKameric 4:147bbe6f9626 437
OsmanKameric 1:2cc1c9a45be7 438 // XnegMD = mpu6050.getXNegMotionDetected();
OsmanKameric 1:2cc1c9a45be7 439 //mpu6050.whoAmI();
OsmanKameric 1:2cc1c9a45be7 440 //uint8_t *data;
OsmanKameric 1:2cc1c9a45be7 441 //char byte = mpu6050.readByte(MPU6050_ADDRESS, 0x61);
OsmanKameric 1:2cc1c9a45be7 442 //*data = byte & (1 << 7);
OsmanKameric 1:2cc1c9a45be7 443 //pc.printf("Byte iznosi: %x\n",byte);
OsmanKameric 1:2cc1c9a45be7 444 //int i = data[0];
OsmanKameric 1:2cc1c9a45be7 445 //pc.printf("7 bit iznosi: %d\n", i);
OsmanKameric 4:147bbe6f9626 446
OsmanKameric 1:2cc1c9a45be7 447 //XposMD = mpu6050.getXPosMotionDetected();
OsmanKameric 4:147bbe6f9626 448
OsmanKameric 4:147bbe6f9626 449
OsmanKameric 1:2cc1c9a45be7 450 //pin.fall(&flip);
OsmanKameric 0:a4de55cab4e2 451 eventQueue.call_every(500, periodicCallback);
OsmanKameric 0:a4de55cab4e2 452 BLE& ble = BLE::Instance();
OsmanKameric 4:147bbe6f9626 453 fsm = new Fsm(ble);
OsmanKameric 0:a4de55cab4e2 454 ble.onEventsToProcess(scheduleBleEventsProcessing);
OsmanKameric 4:147bbe6f9626 455 ble.init(bleInitComplete);
OsmanKameric 0:a4de55cab4e2 456 eventQueue.dispatch_forever();
OsmanKameric 4:147bbe6f9626 457
OsmanKameric 4:147bbe6f9626 458
OsmanKameric 4:147bbe6f9626 459 while (ble.hasInitialized() == false) {
OsmanKameric 4:147bbe6f9626 460 /* spin loop */
OsmanKameric 4:147bbe6f9626 461
OsmanKameric 4:147bbe6f9626 462 }
OsmanKameric 4:147bbe6f9626 463
OsmanKameric 4:147bbe6f9626 464
OsmanKameric 4:147bbe6f9626 465
OsmanKameric 4:147bbe6f9626 466
OsmanKameric 4:147bbe6f9626 467
OsmanKameric 4:147bbe6f9626 468
OsmanKameric 4:147bbe6f9626 469 // pc1.baud(9600);
OsmanKameric 3:b2087af18efe 470 //while (true) {
OsmanKameric 4:147bbe6f9626 471 //ble.waitForEvent();
OsmanKameric 4:147bbe6f9626 472 // pc1.printf("tare ");
OsmanKameric 4:147bbe6f9626 473 //wait_ms(1000);
OsmanKameric 4:147bbe6f9626 474
OsmanKameric 3:b2087af18efe 475 //}
OsmanKameric 3:b2087af18efe 476 //return 0;
OsmanKameric 0:a4de55cab4e2 477 }