ECE4333 - 2018 - Ahmed & Brandon / Mbed OS PlayBack

PlayBack

Dependencies:   TPixy-Interface

Fork of ManualControlFinal by ECE4333 - 2018 - Ahmed & Brandon

CameraThread.cpp@15:cf67f83d5409, 2018-03-03 (annotated)

Committer:
asobhy
Date:
Sat Mar 03 04:47:26 2018 +0000
Revision:
15:cf67f83d5409
Parent:
14:5777377537a2
Child:
16:58ec2b891a25
following robot almost done. Need to get a better object to follow. Need to use a torch to light up the object. Need to adjust width of object in camera settings to match new object then test.

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 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 10:8919b1b76243 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 10:8919b1b76243 92 // signature 0 is cloudy day
asobhy 10:8919b1b76243 93 // signature 1 is indoor lighting
asobhy 15:cf67f83d5409 94
asobhy 15:cf67f83d5409 95 cameraData_mutex.lock();
asobhy 10:8919b1b76243 96 xR = pixyR.blocks[SIGNATURE_IN_USE].x;
asobhy 10:8919b1b76243 97 yR = pixyR.blocks[SIGNATURE_IN_USE].y;
asobhy 10:8919b1b76243 98 ObjectWidth = pixyR.blocks[SIGNATURE_IN_USE].width;
asobhy 10:8919b1b76243 99 ObjectHeight = pixyR.blocks[SIGNATURE_IN_USE].height;
asobhy 10:8919b1b76243 100 ObjectArea = ObjectHeight * ObjectWidth;
asobhy 10:8919b1b76243 101
asobhy 14:5777377537a2 102
asobhy 15:cf67f83d5409 103 DistanceError = DISTANCE - ObjectWidth;
asobhy 15:cf67f83d5409 104 SteeringError = CENTER - xR;
asobhy 15:cf67f83d5409 105 cameraData_mutex.unlock();
asobhy 10:8919b1b76243 106
asobhy 10:8919b1b76243 107 }
asobhy 15:cf67f83d5409 108 // ball not found stop robot from moving
asobhy 15:cf67f83d5409 109 else
asobhy 15:cf67f83d5409 110 {
asobhy 15:cf67f83d5409 111 // set Distance error = 0
asobhy 15:cf67f83d5409 112 DistanceError = 0;
asobhy 15:cf67f83d5409 113 SteeringError = 0;
asobhy 15:cf67f83d5409 114 }
asobhy 15:cf67f83d5409 115
asobhy 10:8919b1b76243 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 * @brief The ISR below signals the CameraThread. it is setup to run
asobhy 10:8919b1b76243 123 * every 20ms
asobhy 10:8919b1b76243 124 * @param none
asobhy 10:8919b1b76243 125 * @return none
asobhy 10:8919b1b76243 126 *******************************************************************************/
asobhy 10:8919b1b76243 127 void CameraPeriodicInterruptISR(void)
asobhy 10:8919b1b76243 128 {
asobhy 10:8919b1b76243 129 // Send signal to the thread with ID, PeriodicInterruptId, i.e., PeriodicInterruptThread.
asobhy 10:8919b1b76243 130 osSignalSet(CameraId,0x1);
asobhy 10:8919b1b76243 131 }
asobhy 10:8919b1b76243 132