Diff: main.cpp

Revision:

17:09ceae7cb00e

Parent:

16:799397f0d3a8

Child:

18:6645cb0f517f

--- a/main.cpp	Tue May 02 23:42:04 2017 +0000
+++ b/main.cpp	Wed May 03 20:41:43 2017 +0000
@@ -93,9 +93,13 @@
 #endif
  
 #define PACKET_SIZE 20
-#define QUEUE_SIZE 20
+#define QUEUE_SIZE 100
 
 #define Z_THRESHOLD 5000
+#define ACCEL_ONE_RESTING 5550
+#define ACCEL_TWO_RESTING 5135
+#define ACCEL_ONE_EPSILON 336
+#define ACCEL_TWO_EPSILON 369
  
 const static char     DEVICE_NAME[] = "LUMBERJACK_NANO";
 static const uint16_t uuid16_list[] = {ButtonService::BUTTON_SERVICE_UUID};
@@ -112,21 +116,23 @@
 packetQueue pq;
  
 void addToQueue(uint8_t* packet) {
-    for (int i = 0; i < PACKET_SIZE; i++) {
-        pq.packets[pq.nextSampleToSave][i] = packet[i];
+    if (pq.nextPacketToSend == pq.nextSampleToSave && pq.liveSamples > 0) {
+        pq.nextPacketToSend = (pq.nextPacketToSend + 1) % QUEUE_SIZE;
+        for (int i = 0; i < PACKET_SIZE; i++) {
+            pq.packets[pq.nextSampleToSave][i] = packet[i];
+        }
+    } else {
+        for (int i = 0; i < PACKET_SIZE; i++) {
+            pq.packets[pq.nextSampleToSave][i] = packet[i];
+        }
+        pq.liveSamples += 1;
     }
-    if (pq.nextPacketToSend == pq.nextSampleToSave && pq.liveSamples > 0) {
-        pq.nextSampleToSave = (pq.nextSampleToSave + 1) % QUEUE_SIZE;
-        pq.nextPacketToSend = (pq.nextPacketToSend + 1) % QUEUE_SIZE;
-    } else {
-        pq.liveSamples += 1;
-        pq.nextSampleToSave = (pq.nextSampleToSave + 1) % QUEUE_SIZE;
-    }
+    pq.nextSampleToSave = (pq.nextSampleToSave + 1) % QUEUE_SIZE;
     return;
 }
  
 uint8_t* removeFromQueue() {
-    if (pq.nextSampleToSave != pq.nextPacketToSend && pq.liveSamples > 0) {
+    if (pq.liveSamples > 0) {
         pq.liveSamples -= 1;
         uint8_t* old = pq.packets[pq.nextPacketToSend];
         pq.nextPacketToSend = (pq.nextPacketToSend + 1) % QUEUE_SIZE;
@@ -164,13 +170,17 @@
 
 void startTransmission() {
     uint8_t* nextPacket = removeFromQueue();
-    buttonServicePtr->updateButtonState(nextPacket);
+    if (nextPacket != NULL) {
+        buttonServicePtr->updateButtonState(nextPacket);
+    }
 }
 
 void dataSentCallback(unsigned count) {
     //pc.printf("dataSent!!\r\n");
     uint8_t* nextPacket = removeFromQueue();
-    buttonServicePtr->updateButtonState(nextPacket);
+    if (nextPacket != NULL) {
+        buttonServicePtr->updateButtonState(nextPacket);
+    }
 }
  
 /**
@@ -445,6 +455,7 @@
     uint8_t lastThreeIndex = 0;
     uint8_t underThresholdCount = 0;
     bool inAKeyStroke = false;
+    uint16_t counter = 0;
    
     while(1)
     {
@@ -459,17 +470,24 @@
        
         //pc.printf("Accel one: x %d y %d z %d\r\n", (int16_t)x1, (int16_t)y1, (int16_t)z1);
         //pc.printf("Accel two: x %d y %d z %d\r\n", (int16_t)x2, (int16_t)y2, (int16_t)z2);
+        bool triggerOne = z1 > ACCEL_ONE_RESTING + ACCEL_ONE_EPSILON || z1 < ACCEL_ONE_RESTING - ACCEL_ONE_EPSILON;
+        bool triggerTwo = z2 > ACCEL_TWO_RESTING + ACCEL_TWO_EPSILON || z2 < ACCEL_TWO_RESTING - ACCEL_TWO_EPSILON;
         uint8_t values[20] = {(uint8_t)(x1 >> 8), (uint8_t)x1, (uint8_t)(y1 >> 8), (uint8_t)y1, (uint8_t)(z1 >> 8), (uint8_t)z1,
                               (uint8_t)(x2 >> 8), (uint8_t)x2, (uint8_t)(y2 >> 8), (uint8_t)y2, (uint8_t)(z2 >> 8), (uint8_t)z2,
-                              0, 0, 0, 0, 0, 0, 0, 0};
+                              (uint8_t)(counter >> 8), (uint8_t) counter, 0, 0, 0, 0, (uint8_t) triggerOne, (uint8_t) triggerTwo};
         //TODO: handle negative accels
-        if (z1 > Z_THRESHOLD || z2 > Z_THRESHOLD) {
+        if (z1 > ACCEL_ONE_RESTING + ACCEL_ONE_EPSILON || z1 < ACCEL_ONE_RESTING - ACCEL_ONE_EPSILON ||
+            z2 > ACCEL_TWO_RESTING + ACCEL_TWO_EPSILON || z2 < ACCEL_TWO_RESTING - ACCEL_TWO_EPSILON) {
             underThresholdCount = 0;
             if (!inAKeyStroke) {
                 //start transmitting
                 inAKeyStroke = true;
                 for (int i = 0; i < 3; i++) {
-                    addToQueue(lastThreePackets[(lastThreeIndex - 1 - i) % 3]);
+                    uint8_t temp[20];
+                    for (int j = 0; j < 20; j++) {
+                        temp[j] = lastThreePackets[(lastThreeIndex + i) % 3][j];
+                    }
+                    addToQueue(temp);
                 }
                 addToQueue(values);
                 startTransmission();
@@ -479,6 +497,7 @@
         } else if (underThresholdCount < 3 && inAKeyStroke) {
             underThresholdCount++;
             addToQueue(values);
+            //todo
         } else {
             for (int i = 0; i < 20; i++) {
                 lastThreePackets[lastThreeIndex][i] = values[i];
@@ -486,8 +505,9 @@
             lastThreeIndex = (lastThreeIndex + 1) % 3;
             inAKeyStroke = false;
         }
+        counter++;
  
-        wait_ms(50);
+        wait_ms(5);
     }
  
 }
\ No newline at end of file