![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
ManualControl
Dependencies: TPixy-Interface
Fork of MbedOS_Robot_Team by
CameraThread.cpp
- Committer:
- asobhy
- Date:
- 2018-03-03
- Revision:
- 14:5777377537a2
- Parent:
- 11:9135e5bc2fcf
- Child:
- 15:cf67f83d5409
File content as of revision 14:5777377537a2:
/******************************************************************************/ // ECE4333 // LAB Partner 1: Ahmed Sobhy - ID: 3594449 // LAB Partner 2: Brandon Kingman - ID: 3470444 // Project: Autonomous Robot Design // Instructor: Prof. Chris Diduch /******************************************************************************/ // filename: CameraThread.cpp // file content description: // * Function to Create the Camera Thread // * CameraThread // * CameraThread ISR // * Variables related to the functionality of the thread /******************************************************************************/ #include "mbed.h" #include "Pixy.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) PixySPI pixyR(&spiR, &bluetooth); void CameraThread(void const *argument); void CameraPeriodicInterruptISR(void); /******************************************************************************/ // osPriorityIdle = -3, ///< priority: idle (lowest) // osPriorityLow = -2, ///< priority: low // osPriorityBelowNormal = -1, ///< priority: below normal // osPriorityNormal = 0, ///< priority: normal (default) // osPriorityAboveNormal = +1, ///< priority: above normal // osPriorityHigh = +2, ///< priority: high // osPriorityRealtime = +3, ///< priority: realtime (highest) /******************************************************************************/ // Declare PeriodicInterruptThread as a thread/process osThreadDef(CameraThread, osPriorityHigh, 1024); Ticker CameraPeriodicInt; // Declare a timer interrupt: PeriodicInt int xR, yR, ObjectWidth, ObjectHeight, ObjectArea, SteeringError, DistanceError; int xRAvg, yRAvg, ObjectWidthAvg, ObjectHeightAvg, ObjectAreaAvg; uint16_t blocksR; /******************************************************************************* * @brief function that creates a thread for the Camera * @param none * @return none *******************************************************************************/ void CameraThreadInit() { pixyR.init(); // Reset all storage xR=0; yR=0; ObjectWidth=0; ObjectHeight=0; 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 } /******************************************************************************* * @brief This is the Camera thread. It reads several values from the * Camera: x coordinate error from center @ 160 & the block area of the object * @param none * @return none *******************************************************************************/ void CameraThread(void const *argument) { static int count = 0; while (true) { osSignalWait(0x01, osWaitForever); // Go to sleep until signal is received. blocksR = pixyR.getBlocks(); 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; // 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; } } } } /******************************************************************************* * @brief The ISR below signals the CameraThread. it is setup to run * every 20ms * @param none * @return none *******************************************************************************/ void CameraPeriodicInterruptISR(void) { // Send signal to the thread with ID, PeriodicInterruptId, i.e., PeriodicInterruptThread. osSignalSet(CameraId,0x1); }