ManualControl
Dependencies: TPixy-Interface
Fork of MbedOS_Robot_Team by
Diff: CameraThread.cpp
- Revision:
- 15:cf67f83d5409
- Parent:
- 14:5777377537a2
- Child:
- 16:58ec2b891a25
diff -r 5777377537a2 -r cf67f83d5409 CameraThread.cpp --- a/CameraThread.cpp Sat Mar 03 02:14:40 2018 +0000 +++ b/CameraThread.cpp Sat Mar 03 04:47:26 2018 +0000 @@ -15,15 +15,13 @@ #include "mbed.h" #include "Pixy.h" +#include "ui.h" #define CENTER 160 #define DISTANCE 20 #define SIGNATURE_IN_USE 0 osThreadId CameraId; - -extern Serial bluetooth; - Mutex cameraData_mutex; SPI spiR(p11, p12, p13); // (mosi, miso, sclk) @@ -69,16 +67,9 @@ ObjectArea=0; 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.05); // 500ms sampling rate + CameraPeriodicInt.attach(&CameraPeriodicInterruptISR, 0.5); // 500ms sampling rate } @@ -90,7 +81,6 @@ *******************************************************************************/ void CameraThread(void const *argument) { - static int count = 0; while (true) { @@ -101,45 +91,28 @@ if (blocksR) { // signature 0 is cloudy day // signature 1 is indoor lighting - + + cameraData_mutex.lock(); 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; - // 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; - } + DistanceError = DISTANCE - ObjectWidth; + SteeringError = CENTER - xR; + cameraData_mutex.unlock(); } + // ball not found stop robot from moving + else + { + // set Distance error = 0 + DistanceError = 0; + SteeringError = 0; + } + }