ECE4333 - 2018 - Ahmed & Brandon / Mbed OS ManualControlFinal

ManualControl

Dependencies:   TPixy-Interface

Fork of MbedOS_Robot_Team by ECE4333 - 2018 - Ahmed & Brandon

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?

UserRevisionLine numberNew 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