ECE4333 - 2018 - Ahmed & Brandon / Mbed OS MbedOS_Robot_Team

ManualControl

Dependencies:   TPixy-Interface

Fork of MbedOS_Robot by ECE4333 - 2018 - Ahmed & Brandon

CameraThread.cpp@29:83c103d12078, 2018-04-10 (annotated)

Committer:
asobhy
Date:
Tue Apr 10 20:54:37 2018 +0000
Branch:
ManualControl
Revision:
29:83c103d12078
Parent:
17:1184df616383 
ManualControl

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