ManualControl
Dependencies: TPixy-Interface
Fork of MbedOS_Robot_Team by
CameraThread.cpp@14:5777377537a2, 2018-03-03 (annotated)
- Committer:
- asobhy
- Date:
- Sat Mar 03 02:14:40 2018 +0000
- Revision:
- 14:5777377537a2
- Parent:
- 11:9135e5bc2fcf
- Child:
- 15:cf67f83d5409
averaging code added to following robot but results are not that good
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
asobhy | 10:8919b1b76243 | 1 | /******************************************************************************/ |
asobhy | 10:8919b1b76243 | 2 | // ECE4333 |
asobhy | 10:8919b1b76243 | 3 | // LAB Partner 1: Ahmed Sobhy - ID: 3594449 |
asobhy | 10:8919b1b76243 | 4 | // LAB Partner 2: Brandon Kingman - ID: 3470444 |
asobhy | 10:8919b1b76243 | 5 | // Project: Autonomous Robot Design |
asobhy | 10:8919b1b76243 | 6 | // Instructor: Prof. Chris Diduch |
asobhy | 10:8919b1b76243 | 7 | /******************************************************************************/ |
asobhy | 10:8919b1b76243 | 8 | // filename: CameraThread.cpp |
asobhy | 10:8919b1b76243 | 9 | // file content description: |
asobhy | 10:8919b1b76243 | 10 | // * Function to Create the Camera Thread |
asobhy | 10:8919b1b76243 | 11 | // * CameraThread |
asobhy | 10:8919b1b76243 | 12 | // * CameraThread ISR |
asobhy | 10:8919b1b76243 | 13 | // * Variables related to the functionality of the thread |
asobhy | 10:8919b1b76243 | 14 | /******************************************************************************/ |
asobhy | 10:8919b1b76243 | 15 | |
asobhy | 10:8919b1b76243 | 16 | #include "mbed.h" |
asobhy | 10:8919b1b76243 | 17 | #include "Pixy.h" |
asobhy | 10:8919b1b76243 | 18 | |
asobhy | 10:8919b1b76243 | 19 | #define CENTER 160 |
asobhy | 10:8919b1b76243 | 20 | #define DISTANCE 20 |
asobhy | 10:8919b1b76243 | 21 | #define SIGNATURE_IN_USE 0 |
asobhy | 10:8919b1b76243 | 22 | |
asobhy | 10:8919b1b76243 | 23 | osThreadId CameraId; |
asobhy | 10:8919b1b76243 | 24 | |
asobhy | 10:8919b1b76243 | 25 | extern Serial bluetooth; |
asobhy | 10:8919b1b76243 | 26 | |
asobhy | 14:5777377537a2 | 27 | Mutex cameraData_mutex; |
asobhy | 14:5777377537a2 | 28 | |
asobhy | 10:8919b1b76243 | 29 | SPI spiR(p11, p12, p13); // (mosi, miso, sclk) |
asobhy | 10:8919b1b76243 | 30 | PixySPI pixyR(&spiR, &bluetooth); |
asobhy | 10:8919b1b76243 | 31 | |
asobhy | 10:8919b1b76243 | 32 | void CameraThread(void const *argument); |
asobhy | 10:8919b1b76243 | 33 | void CameraPeriodicInterruptISR(void); |
asobhy | 10:8919b1b76243 | 34 | |
asobhy | 10:8919b1b76243 | 35 | /******************************************************************************/ |
asobhy | 10:8919b1b76243 | 36 | // osPriorityIdle = -3, ///< priority: idle (lowest) |
asobhy | 10:8919b1b76243 | 37 | // osPriorityLow = -2, ///< priority: low |
asobhy | 10:8919b1b76243 | 38 | // osPriorityBelowNormal = -1, ///< priority: below normal |
asobhy | 10:8919b1b76243 | 39 | // osPriorityNormal = 0, ///< priority: normal (default) |
asobhy | 10:8919b1b76243 | 40 | // osPriorityAboveNormal = +1, ///< priority: above normal |
asobhy | 10:8919b1b76243 | 41 | // osPriorityHigh = +2, ///< priority: high |
asobhy | 10:8919b1b76243 | 42 | // osPriorityRealtime = +3, ///< priority: realtime (highest) |
asobhy | 10:8919b1b76243 | 43 | /******************************************************************************/ |
asobhy | 10:8919b1b76243 | 44 | |
asobhy | 10:8919b1b76243 | 45 | // Declare PeriodicInterruptThread as a thread/process |
asobhy | 10:8919b1b76243 | 46 | osThreadDef(CameraThread, osPriorityHigh, 1024); |
asobhy | 10:8919b1b76243 | 47 | |
asobhy | 10:8919b1b76243 | 48 | Ticker CameraPeriodicInt; // Declare a timer interrupt: PeriodicInt |
asobhy | 10:8919b1b76243 | 49 | |
asobhy | 10:8919b1b76243 | 50 | int xR, yR, ObjectWidth, ObjectHeight, ObjectArea, SteeringError, DistanceError; |
asobhy | 14:5777377537a2 | 51 | int xRAvg, yRAvg, ObjectWidthAvg, ObjectHeightAvg, ObjectAreaAvg; |
asobhy | 14:5777377537a2 | 52 | |
asobhy | 10:8919b1b76243 | 53 | uint16_t blocksR; |
asobhy | 10:8919b1b76243 | 54 | |
asobhy | 10:8919b1b76243 | 55 | /******************************************************************************* |
asobhy | 10:8919b1b76243 | 56 | * @brief function that creates a thread for the Camera |
asobhy | 10:8919b1b76243 | 57 | * @param none |
asobhy | 10:8919b1b76243 | 58 | * @return none |
asobhy | 10:8919b1b76243 | 59 | *******************************************************************************/ |
asobhy | 10:8919b1b76243 | 60 | void CameraThreadInit() |
asobhy | 10:8919b1b76243 | 61 | { |
asobhy | 10:8919b1b76243 | 62 | pixyR.init(); |
asobhy | 14:5777377537a2 | 63 | |
asobhy | 14:5777377537a2 | 64 | // Reset all storage |
asobhy | 10:8919b1b76243 | 65 | xR=0; |
asobhy | 10:8919b1b76243 | 66 | yR=0; |
asobhy | 10:8919b1b76243 | 67 | ObjectWidth=0; |
asobhy | 10:8919b1b76243 | 68 | ObjectHeight=0; |
asobhy | 10:8919b1b76243 | 69 | ObjectArea=0; |
asobhy | 10:8919b1b76243 | 70 | SteeringError=0; |
asobhy | 10:8919b1b76243 | 71 | DistanceError=0; |
asobhy | 10:8919b1b76243 | 72 | |
asobhy | 14:5777377537a2 | 73 | // Reset all storage |
asobhy | 14:5777377537a2 | 74 | xRAvg=0; |
asobhy | 14:5777377537a2 | 75 | yRAvg=0; |
asobhy | 14:5777377537a2 | 76 | ObjectWidthAvg=0; |
asobhy | 14:5777377537a2 | 77 | ObjectHeightAvg=0; |
asobhy | 14:5777377537a2 | 78 | ObjectAreaAvg=0; |
asobhy | 14:5777377537a2 | 79 | |
asobhy | 10:8919b1b76243 | 80 | CameraId = osThreadCreate(osThread(CameraThread), NULL); |
asobhy | 14:5777377537a2 | 81 | CameraPeriodicInt.attach(&CameraPeriodicInterruptISR, 0.05); // 500ms sampling rate |
asobhy | 10:8919b1b76243 | 82 | } |
asobhy | 10:8919b1b76243 | 83 | |
asobhy | 10:8919b1b76243 | 84 | |
asobhy | 10:8919b1b76243 | 85 | /******************************************************************************* |
asobhy | 10:8919b1b76243 | 86 | * @brief This is the Camera thread. It reads several values from the |
asobhy | 10:8919b1b76243 | 87 | * Camera: x coordinate error from center @ 160 & the block area of the object |
asobhy | 10:8919b1b76243 | 88 | * @param none |
asobhy | 10:8919b1b76243 | 89 | * @return none |
asobhy | 10:8919b1b76243 | 90 | *******************************************************************************/ |
asobhy | 10:8919b1b76243 | 91 | void CameraThread(void const *argument) |
asobhy | 10:8919b1b76243 | 92 | { |
asobhy | 14:5777377537a2 | 93 | static int count = 0; |
asobhy | 10:8919b1b76243 | 94 | |
asobhy | 10:8919b1b76243 | 95 | while (true) |
asobhy | 10:8919b1b76243 | 96 | { |
asobhy | 10:8919b1b76243 | 97 | |
asobhy | 10:8919b1b76243 | 98 | osSignalWait(0x01, osWaitForever); // Go to sleep until signal is received. |
asobhy | 10:8919b1b76243 | 99 | |
asobhy | 10:8919b1b76243 | 100 | blocksR = pixyR.getBlocks(); |
asobhy | 10:8919b1b76243 | 101 | if (blocksR) { |
asobhy | 10:8919b1b76243 | 102 | // signature 0 is cloudy day |
asobhy | 10:8919b1b76243 | 103 | // signature 1 is indoor lighting |
asobhy | 14:5777377537a2 | 104 | |
asobhy | 10:8919b1b76243 | 105 | xR = pixyR.blocks[SIGNATURE_IN_USE].x; |
asobhy | 10:8919b1b76243 | 106 | yR = pixyR.blocks[SIGNATURE_IN_USE].y; |
asobhy | 10:8919b1b76243 | 107 | ObjectWidth = pixyR.blocks[SIGNATURE_IN_USE].width; |
asobhy | 10:8919b1b76243 | 108 | ObjectHeight = pixyR.blocks[SIGNATURE_IN_USE].height; |
asobhy | 10:8919b1b76243 | 109 | ObjectArea = ObjectHeight * ObjectWidth; |
asobhy | 10:8919b1b76243 | 110 | |
asobhy | 14:5777377537a2 | 111 | // Accumulate readings to be used for average value calculation |
asobhy | 14:5777377537a2 | 112 | xRAvg += xR; |
asobhy | 14:5777377537a2 | 113 | yRAvg += yR; |
asobhy | 14:5777377537a2 | 114 | ObjectWidthAvg += ObjectWidth; |
asobhy | 14:5777377537a2 | 115 | ObjectHeightAvg += ObjectHeight; |
asobhy | 14:5777377537a2 | 116 | ObjectAreaAvg += ObjectArea; |
asobhy | 14:5777377537a2 | 117 | |
asobhy | 14:5777377537a2 | 118 | count++; |
asobhy | 14:5777377537a2 | 119 | // Average calculation 10 readings |
asobhy | 14:5777377537a2 | 120 | if(count > 10) |
asobhy | 14:5777377537a2 | 121 | { |
asobhy | 14:5777377537a2 | 122 | xRAvg=xRAvg/10; |
asobhy | 14:5777377537a2 | 123 | yRAvg=yRAvg/10; |
asobhy | 14:5777377537a2 | 124 | ObjectWidthAvg=ObjectWidthAvg/10; |
asobhy | 14:5777377537a2 | 125 | ObjectHeightAvg=ObjectHeightAvg/10; |
asobhy | 14:5777377537a2 | 126 | ObjectAreaAvg=ObjectAreaAvg/10; |
asobhy | 14:5777377537a2 | 127 | |
asobhy | 14:5777377537a2 | 128 | cameraData_mutex.lock(); |
asobhy | 14:5777377537a2 | 129 | DistanceError = DISTANCE - ObjectWidthAvg; |
asobhy | 14:5777377537a2 | 130 | SteeringError = CENTER - xRAvg; |
asobhy | 14:5777377537a2 | 131 | cameraData_mutex.unlock(); |
asobhy | 14:5777377537a2 | 132 | |
asobhy | 14:5777377537a2 | 133 | count = 0; |
asobhy | 14:5777377537a2 | 134 | // Reset all storage |
asobhy | 14:5777377537a2 | 135 | xRAvg=0; |
asobhy | 14:5777377537a2 | 136 | yRAvg=0; |
asobhy | 14:5777377537a2 | 137 | ObjectWidthAvg=0; |
asobhy | 14:5777377537a2 | 138 | ObjectHeightAvg=0; |
asobhy | 14:5777377537a2 | 139 | ObjectAreaAvg=0; |
asobhy | 14:5777377537a2 | 140 | } |
asobhy | 10:8919b1b76243 | 141 | |
asobhy | 10:8919b1b76243 | 142 | } |
asobhy | 10:8919b1b76243 | 143 | |
asobhy | 10:8919b1b76243 | 144 | } |
asobhy | 10:8919b1b76243 | 145 | |
asobhy | 10:8919b1b76243 | 146 | } |
asobhy | 10:8919b1b76243 | 147 | |
asobhy | 10:8919b1b76243 | 148 | /******************************************************************************* |
asobhy | 10:8919b1b76243 | 149 | * @brief The ISR below signals the CameraThread. it is setup to run |
asobhy | 10:8919b1b76243 | 150 | * every 20ms |
asobhy | 10:8919b1b76243 | 151 | * @param none |
asobhy | 10:8919b1b76243 | 152 | * @return none |
asobhy | 10:8919b1b76243 | 153 | *******************************************************************************/ |
asobhy | 10:8919b1b76243 | 154 | void CameraPeriodicInterruptISR(void) |
asobhy | 10:8919b1b76243 | 155 | { |
asobhy | 10:8919b1b76243 | 156 | // Send signal to the thread with ID, PeriodicInterruptId, i.e., PeriodicInterruptThread. |
asobhy | 10:8919b1b76243 | 157 | osSignalSet(CameraId,0x1); |
asobhy | 10:8919b1b76243 | 158 | } |
asobhy | 10:8919b1b76243 | 159 |