ObjectFollower
Dependencies: TPixy-Interface
Fork of PlayBack by
CameraThread.cpp
- Committer:
- asobhy
- Date:
- 2018-03-11
- Revision:
- 17:1184df616383
- Parent:
- 16:58ec2b891a25
File content as of revision 17:1184df616383:
/******************************************************************************/ // 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" #include "ui.h" #define CENTER 160 #define DISTANCE 20 #define SIGNATURE_IN_USE 0 osThreadId CameraId; 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; CameraId = osThreadCreate(osThread(CameraThread), NULL); CameraPeriodicInt.attach(&CameraPeriodicInterruptISR, 0.5); // 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) { 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 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; 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; } } } /******************************************************************************* * @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); }