![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
ManualControl
Dependencies: TPixy-Interface
Fork of MbedOS_Robot_Team by
CameraThread.cpp@17:1184df616383, 2018-03-11 (annotated)
- Committer:
- asobhy
- Date:
- Sun Mar 11 01:26:23 2018 +0000
- Revision:
- 17:1184df616383
- Parent:
- 16:58ec2b891a25
MANUAL CONTROL - TESTING AND TUNNING REQUIRED
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 | 15:cf67f83d5409 | 18 | #include "ui.h" |
asobhy | 10:8919b1b76243 | 19 | |
asobhy | 10:8919b1b76243 | 20 | #define CENTER 160 |
asobhy | 10:8919b1b76243 | 21 | #define DISTANCE 20 |
asobhy | 10:8919b1b76243 | 22 | #define SIGNATURE_IN_USE 0 |
asobhy | 10:8919b1b76243 | 23 | |
asobhy | 10:8919b1b76243 | 24 | osThreadId CameraId; |
asobhy | 14:5777377537a2 | 25 | Mutex cameraData_mutex; |
asobhy | 14:5777377537a2 | 26 | |
asobhy | 10:8919b1b76243 | 27 | SPI spiR(p11, p12, p13); // (mosi, miso, sclk) |
asobhy | 10:8919b1b76243 | 28 | PixySPI pixyR(&spiR, &bluetooth); |
asobhy | 10:8919b1b76243 | 29 | |
asobhy | 10:8919b1b76243 | 30 | void CameraThread(void const *argument); |
asobhy | 10:8919b1b76243 | 31 | void CameraPeriodicInterruptISR(void); |
asobhy | 10:8919b1b76243 | 32 | |
asobhy | 10:8919b1b76243 | 33 | /******************************************************************************/ |
asobhy | 10:8919b1b76243 | 34 | // osPriorityIdle = -3, ///< priority: idle (lowest) |
asobhy | 10:8919b1b76243 | 35 | // osPriorityLow = -2, ///< priority: low |
asobhy | 10:8919b1b76243 | 36 | // osPriorityBelowNormal = -1, ///< priority: below normal |
asobhy | 10:8919b1b76243 | 37 | // osPriorityNormal = 0, ///< priority: normal (default) |
asobhy | 10:8919b1b76243 | 38 | // osPriorityAboveNormal = +1, ///< priority: above normal |
asobhy | 10:8919b1b76243 | 39 | // osPriorityHigh = +2, ///< priority: high |
asobhy | 10:8919b1b76243 | 40 | // osPriorityRealtime = +3, ///< priority: realtime (highest) |
asobhy | 10:8919b1b76243 | 41 | /******************************************************************************/ |
asobhy | 10:8919b1b76243 | 42 | |
asobhy | 10:8919b1b76243 | 43 | // Declare PeriodicInterruptThread as a thread/process |
asobhy | 10:8919b1b76243 | 44 | osThreadDef(CameraThread, osPriorityHigh, 1024); |
asobhy | 10:8919b1b76243 | 45 | |
asobhy | 10:8919b1b76243 | 46 | Ticker CameraPeriodicInt; // Declare a timer interrupt: PeriodicInt |
asobhy | 10:8919b1b76243 | 47 | |
asobhy | 10:8919b1b76243 | 48 | int xR, yR, ObjectWidth, ObjectHeight, ObjectArea, SteeringError, DistanceError; |
asobhy | 14:5777377537a2 | 49 | int xRAvg, yRAvg, ObjectWidthAvg, ObjectHeightAvg, ObjectAreaAvg; |
asobhy | 14:5777377537a2 | 50 | |
asobhy | 10:8919b1b76243 | 51 | uint16_t blocksR; |
asobhy | 10:8919b1b76243 | 52 | |
asobhy | 10:8919b1b76243 | 53 | /******************************************************************************* |
asobhy | 10:8919b1b76243 | 54 | * @brief function that creates a thread for the Camera |
asobhy | 10:8919b1b76243 | 55 | * @param none |
asobhy | 10:8919b1b76243 | 56 | * @return none |
asobhy | 10:8919b1b76243 | 57 | *******************************************************************************/ |
asobhy | 10:8919b1b76243 | 58 | void CameraThreadInit() |
asobhy | 10:8919b1b76243 | 59 | { |
asobhy | 10:8919b1b76243 | 60 | pixyR.init(); |
asobhy | 14:5777377537a2 | 61 | |
asobhy | 14:5777377537a2 | 62 | // Reset all storage |
asobhy | 10:8919b1b76243 | 63 | xR=0; |
asobhy | 10:8919b1b76243 | 64 | yR=0; |
asobhy | 10:8919b1b76243 | 65 | ObjectWidth=0; |
asobhy | 10:8919b1b76243 | 66 | ObjectHeight=0; |
asobhy | 10:8919b1b76243 | 67 | ObjectArea=0; |
asobhy | 10:8919b1b76243 | 68 | SteeringError=0; |
asobhy | 10:8919b1b76243 | 69 | DistanceError=0; |
asobhy | 14:5777377537a2 | 70 | |
asobhy | 10:8919b1b76243 | 71 | CameraId = osThreadCreate(osThread(CameraThread), NULL); |
asobhy | 15:cf67f83d5409 | 72 | CameraPeriodicInt.attach(&CameraPeriodicInterruptISR, 0.5); // 500ms sampling rate |
asobhy | 10:8919b1b76243 | 73 | } |
asobhy | 10:8919b1b76243 | 74 | |
asobhy | 10:8919b1b76243 | 75 | |
asobhy | 10:8919b1b76243 | 76 | /******************************************************************************* |
asobhy | 10:8919b1b76243 | 77 | * @brief This is the Camera thread. It reads several values from the |
asobhy | 10:8919b1b76243 | 78 | * Camera: x coordinate error from center @ 160 & the block area of the object |
asobhy | 10:8919b1b76243 | 79 | * @param none |
asobhy | 10:8919b1b76243 | 80 | * @return none |
asobhy | 10:8919b1b76243 | 81 | *******************************************************************************/ |
asobhy | 10:8919b1b76243 | 82 | void CameraThread(void const *argument) |
asobhy | 10:8919b1b76243 | 83 | { |
asobhy | 10:8919b1b76243 | 84 | |
asobhy | 10:8919b1b76243 | 85 | while (true) |
asobhy | 10:8919b1b76243 | 86 | { |
asobhy | 17:1184df616383 | 87 | |
asobhy | 10:8919b1b76243 | 88 | osSignalWait(0x01, osWaitForever); // Go to sleep until signal is received. |
asobhy | 10:8919b1b76243 | 89 | |
asobhy | 10:8919b1b76243 | 90 | blocksR = pixyR.getBlocks(); |
asobhy | 10:8919b1b76243 | 91 | if (blocksR) { |
asobhy | 16:58ec2b891a25 | 92 | |
asobhy | 10:8919b1b76243 | 93 | // signature 0 is cloudy day |
asobhy | 10:8919b1b76243 | 94 | // signature 1 is indoor lighting |
asobhy | 15:cf67f83d5409 | 95 | |
asobhy | 15:cf67f83d5409 | 96 | cameraData_mutex.lock(); |
asobhy | 10:8919b1b76243 | 97 | xR = pixyR.blocks[SIGNATURE_IN_USE].x; |
asobhy | 10:8919b1b76243 | 98 | yR = pixyR.blocks[SIGNATURE_IN_USE].y; |
asobhy | 10:8919b1b76243 | 99 | ObjectWidth = pixyR.blocks[SIGNATURE_IN_USE].width; |
asobhy | 10:8919b1b76243 | 100 | ObjectHeight = pixyR.blocks[SIGNATURE_IN_USE].height; |
asobhy | 10:8919b1b76243 | 101 | ObjectArea = ObjectHeight * ObjectWidth; |
asobhy | 10:8919b1b76243 | 102 | |
asobhy | 14:5777377537a2 | 103 | |
asobhy | 15:cf67f83d5409 | 104 | DistanceError = DISTANCE - ObjectWidth; |
asobhy | 15:cf67f83d5409 | 105 | SteeringError = CENTER - xR; |
asobhy | 15:cf67f83d5409 | 106 | cameraData_mutex.unlock(); |
asobhy | 10:8919b1b76243 | 107 | |
asobhy | 10:8919b1b76243 | 108 | } |
asobhy | 15:cf67f83d5409 | 109 | // ball not found stop robot from moving |
asobhy | 15:cf67f83d5409 | 110 | else |
asobhy | 15:cf67f83d5409 | 111 | { |
asobhy | 15:cf67f83d5409 | 112 | // set Distance error = 0 |
asobhy | 15:cf67f83d5409 | 113 | DistanceError = 0; |
asobhy | 15:cf67f83d5409 | 114 | SteeringError = 0; |
asobhy | 15:cf67f83d5409 | 115 | } |
asobhy | 15:cf67f83d5409 | 116 | |
asobhy | 10:8919b1b76243 | 117 | |
asobhy | 10:8919b1b76243 | 118 | } |
asobhy | 10:8919b1b76243 | 119 | |
asobhy | 10:8919b1b76243 | 120 | } |
asobhy | 10:8919b1b76243 | 121 | |
asobhy | 10:8919b1b76243 | 122 | /******************************************************************************* |
asobhy | 10:8919b1b76243 | 123 | * @brief The ISR below signals the CameraThread. it is setup to run |
asobhy | 10:8919b1b76243 | 124 | * every 20ms |
asobhy | 10:8919b1b76243 | 125 | * @param none |
asobhy | 10:8919b1b76243 | 126 | * @return none |
asobhy | 10:8919b1b76243 | 127 | *******************************************************************************/ |
asobhy | 10:8919b1b76243 | 128 | void CameraPeriodicInterruptISR(void) |
asobhy | 10:8919b1b76243 | 129 | { |
asobhy | 10:8919b1b76243 | 130 | // Send signal to the thread with ID, PeriodicInterruptId, i.e., PeriodicInterruptThread. |
asobhy | 10:8919b1b76243 | 131 | osSignalSet(CameraId,0x1); |
asobhy | 10:8919b1b76243 | 132 | } |
asobhy | 10:8919b1b76243 | 133 |