![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
ManualControl
Dependencies: TPixy-Interface
Fork of MbedOS_Robot_Team by
Diff: CameraThread.cpp
- Revision:
- 10:8919b1b76243
- Child:
- 11:9135e5bc2fcf
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/CameraThread.cpp Fri Mar 02 23:37:31 2018 +0000 @@ -0,0 +1,116 @@ +/******************************************************************************/ +// 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; + +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; +uint16_t blocksR; + +/******************************************************************************* +* @brief function that creates a thread for the Camera +* @param none +* @return none +*******************************************************************************/ +void CameraThreadInit() +{ + pixyR.init(); + xR=0; + yR=0; + ObjectWidth=0; + ObjectHeight=0; + ObjectArea=0; + SteeringError=0; + DistanceError=0; + + CameraId = osThreadCreate(osThread(CameraThread), NULL); + CameraPeriodicInt.attach(&CameraPeriodicInterruptISR, 0.02); // 20ms sampling rate - 50fps +} + + +/******************************************************************************* +* @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 + 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; + + } + + } + +} + +/******************************************************************************* +* @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); +} +