Diff: CameraThread.cpp

Revision:

14:5777377537a2

Parent:

11:9135e5bc2fcf

Child:

15:cf67f83d5409

--- a/CameraThread.cpp	Sat Mar 03 00:53:06 2018 +0000
+++ b/CameraThread.cpp	Sat Mar 03 02:14:40 2018 +0000
@@ -24,6 +24,8 @@
 
 extern Serial bluetooth;
 
+Mutex cameraData_mutex;
+
 SPI spiR(p11, p12, p13); // (mosi, miso, sclk)
 PixySPI pixyR(&spiR, &bluetooth);
 
@@ -46,6 +48,8 @@
 Ticker CameraPeriodicInt;      // Declare a timer interrupt: PeriodicInt
 
 int xR, yR, ObjectWidth, ObjectHeight, ObjectArea, SteeringError, DistanceError;
+int xRAvg, yRAvg, ObjectWidthAvg, ObjectHeightAvg, ObjectAreaAvg;
+
 uint16_t blocksR;
 
 /*******************************************************************************
@@ -56,6 +60,8 @@
 void CameraThreadInit()
 {
     pixyR.init();
+
+    // Reset all storage
     xR=0;
     yR=0;
     ObjectWidth=0;
@@ -64,8 +70,15 @@
     SteeringError=0; 
     DistanceError=0;
     
+    // Reset all storage
+    xRAvg=0;
+    yRAvg=0; 
+    ObjectWidthAvg=0; 
+    ObjectHeightAvg=0; 
+    ObjectAreaAvg=0; 
+
     CameraId = osThreadCreate(osThread(CameraThread), NULL);
-    CameraPeriodicInt.attach(&CameraPeriodicInterruptISR, 0.5); // 500ms sampling rate    
+    CameraPeriodicInt.attach(&CameraPeriodicInterruptISR, 0.05); // 500ms sampling rate    
 }
 
 
@@ -77,6 +90,7 @@
 *******************************************************************************/
 void CameraThread(void const *argument)
 {
+    static int count = 0;
     
     while (true) 
     {
@@ -87,14 +101,43 @@
         if (blocksR) {
             // signature 0 is cloudy day
             // signature 1 is indoor lighting
+             
             xR = pixyR.blocks[SIGNATURE_IN_USE].x;
             yR = pixyR.blocks[SIGNATURE_IN_USE].y;
             ObjectWidth = pixyR.blocks[SIGNATURE_IN_USE].width;
             ObjectHeight = pixyR.blocks[SIGNATURE_IN_USE].height;
             ObjectArea = ObjectHeight * ObjectWidth;
             
-            DistanceError = DISTANCE - ObjectWidth;
-            SteeringError = CENTER - xR;
+            // Accumulate readings to be used for average value calculation
+            xRAvg += xR;
+            yRAvg += yR; 
+            ObjectWidthAvg += ObjectWidth; 
+            ObjectHeightAvg += ObjectHeight; 
+            ObjectAreaAvg += ObjectArea; 
+            
+            count++;
+            // Average calculation 10 readings
+            if(count > 10)
+            {   
+                xRAvg=xRAvg/10;
+                yRAvg=yRAvg/10; 
+                ObjectWidthAvg=ObjectWidthAvg/10; 
+                ObjectHeightAvg=ObjectHeightAvg/10; 
+                ObjectAreaAvg=ObjectAreaAvg/10; 
+            
+                cameraData_mutex.lock();
+                DistanceError = DISTANCE - ObjectWidthAvg;
+                SteeringError = CENTER - xRAvg;
+                cameraData_mutex.unlock();
+                
+                count = 0;
+                    // Reset all storage
+    xRAvg=0;
+    yRAvg=0; 
+    ObjectWidthAvg=0; 
+    ObjectHeightAvg=0; 
+    ObjectAreaAvg=0;
+            }
             
         }