Diff: main.cpp

Revision:

1:1c14c1d3ce09

Parent:

0:4c7b37b8faad

Child:

2:eb47002f16b5

--- a/main.cpp	Fri Jan 08 20:41:56 2016 +0000
+++ b/main.cpp	Tue Feb 09 19:45:55 2016 +0000
@@ -1,58 +1,161 @@
-
 #include "mbed.h"
 #include "BLEDevice.h"
-#include "ADXL345.h"
+#include "MMA8452.h"
+
+// Configuration
+#define BLE_ADV_INTERVAL    500         // Interval between each advertising packet (in MS)
+#define ACC_RATE            10          // Accelerometer sampling rate (in Hz)
+#define ACC_BUFFER_SIZE     10          // Accelerometer history size
 
-// The accelerometer sample rate defines how often advertising
-// packets need to be sent. Each packet can hold 9 samples.
-#define ACC_SAMPLE_RATE     9
+// Algorithm
+#define ALG_PERIOD          1           // Algorithm period
+#define ALG_ACC_AXIS        0           // Which accelerometer axis to use
+#define ALG_UP_THRES        0.3         // Up State Threshold
+#define ALG_UP_TTCKS        10          // Up State Trigger Time
+#define ALG_DOWN_THRES      0.3         // Down State Threshold
+#define ALG_DOWN_TICKS      4           // Down State Trigger Time
 
-// How many times to re-advertise the same packet. For example,
-// if for the given sample rate an advertising packet needs to
-// be sent every second, and ADV_RESEND_COUNT of 2 will send
-// 2 packets per second.
-#define ADV_RESEND_COUNT    10
+// Objects
+BLEDevice   ble;                        // BLE
+DigitalOut  led(P0_2);                  // LED (Active Low)
+InterruptIn btn(P0_3);                  // Button Interrupt (Active Low)
+MMA8452     acc(P0_22, P0_20, 100000);  // Accelerometer
+Ticker      acc_tick;                   // Ticker for the accelerometer
+Ticker      led_tick;                   // Ticker for led
+Ticker      alg_tick;                   // Ticker for algorithm
+
+// Data
+struct acc_ring {
+    double data[3][ACC_BUFFER_SIZE];
+    int head;
+} acc_ring;
+
+enum AlgState {
+    UP,
+    DOWN
+};
 
-BLEDevice   ble;
-ADXL345     acc(P0_9, P0_11, P0_8, P0_10);
-DigitalOut  led(P0_19);
+// Prototypes
+void onError();
+void onAccTick();
+void onButton();
+void onLedTick();
+void ledBlink(int, float);
+void ledStop();
+double getAccLast(int);
+double getAccMean(int);
+void setPayload(uint8_t*, uint8_t);
+
+// Events
+void onError() {
+    led = 1;
+    wait(0.25);
+    led = 0;
+    wait(0.25);
+}
 
-void accRead() {
-    int data[3];
-    static int8_t buffer[28];
-    static uint8_t index = 0;
-    static uint8_t count;
+void onAccTick() {
+    // Increment head
+    acc_ring.head++;
+    if (acc_ring.head >= ACC_BUFFER_SIZE) acc_ring.head = 0;
     
-    acc.getOutput(data);
+    // Read accelerometer
+    acc.readXGravity(&acc_ring.data[0][acc_ring.head]);
+    acc.readYGravity(&acc_ring.data[1][acc_ring.head]);
+    acc.readZGravity(&acc_ring.data[2][acc_ring.head]);
+}
+
+void onButton() {
+    ledBlink(3, 1);
+}
+
+int led_blink;
+void onLedTick() {
+    led = !led;         // Invert LED state
     
-    // Fill buffer
-    buffer[index] = (int8_t)(data[0] >> 8);
-    buffer[index+1] = (int8_t)(data[1] >> 8);
-    buffer[index+2] = (int8_t)(data[2] >> 8);
-    index += 3;
+    if (led == 1)       // If led was turned off
+        led_blink--;    // Decrement blink counter
+        
+    if (led_blink == 0)
+        led_tick.detach();
+}
+
+void onAlgTick() {
+    static AlgState state = UP;
+    static AlgState last = DOWN;
+    static int timer;
+    
+    double pos = getAccMean(ALG_ACC_AXIS);
+    
+    switch (state) {
+        case UP:
+            if (pos < ALG_DOWN_THRES) {
+                timer = 0;
+            } else {
+                ledBlink(1, 1);
+            }
+            if (timer > ALG_DOWN_TICKS) {
+                state = DOWN;
+            }
+            break;
+            
+        case DOWN:
+            if (pos > ALG_UP_THRES) {
+                timer = 0;
+            } else {
+                ledBlink(1, 1);
+            }
+            if (timer > 10) {
+                state = UP;
+            }
+            break;
+    }
     
-    // If buffer is full (9 readings) update advertising packet
-    if (index >= 27) {
-        index = 0;                      // Reset buffer index
-        count += 1;                     // Increase packet count
-        buffer[27] = count;             // Append packet count
-        ble.clearScanResponse();        // Clear advertising packet and add data
-        ble.accumulateScanResponse(GapAdvertisingData::MANUFACTURER_SPECIFIC_DATA, (uint8_t*)buffer, sizeof(buffer));
-        ble.setAdvertisingPayload();
-        led = 0;                        // Flash led
-        wait(0.01);
-        led = 1;
+    if (state != last) {
+        last = state;
+        setPayload((uint8_t*)&state, 1);
+        ledBlink(2, 0.25);
     }
+    
+    timer++;
 }
-    
-    
+
+// Functions
+void setPayload(uint8_t * data, uint8_t size) {
+    ble.clearScanResponse();
+    ble.accumulateScanResponse(GapAdvertisingData::MANUFACTURER_SPECIFIC_DATA, data, size);
+    ble.setAdvertisingPayload();
+}
+
+void ledBlink(int num, float period) {
+    led = 0;
+    led_blink = num;
+    led_tick.attach(&onLedTick, period/2);
+}
+
+void ledStop() {
+    led_blink = 0;
+    led_tick.detach();
+}
+
+double getAccLast(int axis) {
+    return acc_ring.data[axis][acc_ring.head];
+}
+
+double getAccMean(int axis) {
+    double mean = 0;
+    for (int i = 0; i < ACC_BUFFER_SIZE; i++) {
+        mean += acc_ring.data[axis][i];
+    }
+    return (mean / (float)ACC_BUFFER_SIZE);
+}
+
+
 int main(void) {
-    // Turn off led
-    led = 0;
-    
-    // Calculate advertising interval
-    uint16_t adv_interval;
-    adv_interval = (uint16_t)((9.0 / (float)ACC_SAMPLE_RATE) * 1000.0 / (float)ADV_RESEND_COUNT);
+    // Initialize LED and Button
+    led = 1;
+    btn.mode(PullUp);
+    btn.fall(&onButton);
     
     // Initialize BLE
     uint8_t tagAddress[6];
@@ -69,18 +172,24 @@
         GapAdvertisingData::COMPLETE_LOCAL_NAME,
         tagName,
         sizeof(tagName));
-    ble.setAdvertisingInterval(adv_interval);               // Advertising interval
+    ble.setAdvertisingInterval((uint16_t)((float)BLE_ADV_INTERVAL / 0.625));    // Advertising interval
     ble.startAdvertising();                                 // Start advertising
     
-    // Setup ticker to read accelerometer
-    acc.setPowerControl(0x00);      // Standby for configuration
-    acc.setDataFormatControl(0x05); // +-4g, left-justified,
-    acc.setDataRate(ADXL345_12HZ5); // 12.5Hz Internal Sample Rate
-    acc.setPowerControl(0x08);      // Measurement mode
-    Ticker acc_read;
-    acc_read.attach(&accRead, (1.0 / (float)ACC_SAMPLE_RATE));
+    // Initialize accelerometer
+    char acc_id;
+    acc.getDeviceID(&acc_id);                               // Check if accelerometer succesfully comunicates
+    if (acc_id != 0x2A) onError();
+    acc.standby();                                          // Put into standby mode before configuration
+    acc.setDataRate(acc.RATE_12_5);                         // Set hardware data rate to 12.5Hz
+    acc.setDynamicRange(acc.DYNAMIC_RANGE_2G);              // Set dynamic range up to 2G
+    acc.setBitDepth(acc.BIT_DEPTH_12);                      // Set bit depth to 12bits for resolution
+    acc.activate();                                         // Activate accelerometer
+    acc_tick.attach(&onAccTick, (1.0 / (float)ACC_RATE));   // Setup periodic reads
     
-    while(1) {
-        ble.waitForEvent();     // Sleep
+    // Setup algorithm
+    alg_tick.attach(&onAlgTick, ALG_PERIOD);
+    
+    while (1) {
+        ble.waitForEvent();
     }
 }