![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
ManualControl
Dependencies: TPixy-Interface
Fork of MbedOS_Robot_Team by
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; + } }