ECE4333 - 2018 - Ahmed & Brandon / Mbed OS MbedOS_Robot_Team

ManualControl

Dependencies:   TPixy-Interface

Fork of MbedOS_Robot by ECE4333 - 2018 - Ahmed & Brandon

CameraThread.cpp@10:8919b1b76243, 2018-03-02 (annotated)

Committer:
asobhy
Date:
Fri Mar 02 23:37:31 2018 +0000
Revision:
10:8919b1b76243
Child:
11:9135e5bc2fcf
vision + robot testing

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 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 10:8919b1b76243 49 uint16_t blocksR;
asobhy 10:8919b1b76243 50
asobhy 10:8919b1b76243 51 /*******************************************************************************
asobhy 10:8919b1b76243 52 * @brief function that creates a thread for the Camera
asobhy 10:8919b1b76243 53 * @param none
asobhy 10:8919b1b76243 54 * @return none
asobhy 10:8919b1b76243 55 *******************************************************************************/
asobhy 10:8919b1b76243 56 void CameraThreadInit()
asobhy 10:8919b1b76243 57 {
asobhy 10:8919b1b76243 58 pixyR.init();
asobhy 10:8919b1b76243 59 xR=0;
asobhy 10:8919b1b76243 60 yR=0;
asobhy 10:8919b1b76243 61 ObjectWidth=0;
asobhy 10:8919b1b76243 62 ObjectHeight=0;
asobhy 10:8919b1b76243 63 ObjectArea=0;
asobhy 10:8919b1b76243 64 SteeringError=0;
asobhy 10:8919b1b76243 65 DistanceError=0;
asobhy 10:8919b1b76243 66
asobhy 10:8919b1b76243 67 CameraId = osThreadCreate(osThread(CameraThread), NULL);
asobhy 10:8919b1b76243 68 CameraPeriodicInt.attach(&CameraPeriodicInterruptISR, 0.02); // 20ms sampling rate - 50fps
asobhy 10:8919b1b76243 69 }
asobhy 10:8919b1b76243 70
asobhy 10:8919b1b76243 71
asobhy 10:8919b1b76243 72 /*******************************************************************************
asobhy 10:8919b1b76243 73 * @brief This is the Camera thread. It reads several values from the
asobhy 10:8919b1b76243 74 * Camera: x coordinate error from center @ 160 & the block area of the object
asobhy 10:8919b1b76243 75 * @param none
asobhy 10:8919b1b76243 76 * @return none
asobhy 10:8919b1b76243 77 *******************************************************************************/
asobhy 10:8919b1b76243 78 void CameraThread(void const *argument)
asobhy 10:8919b1b76243 79 {
asobhy 10:8919b1b76243 80
asobhy 10:8919b1b76243 81 while (true)
asobhy 10:8919b1b76243 82 {
asobhy 10:8919b1b76243 83
asobhy 10:8919b1b76243 84 osSignalWait(0x01, osWaitForever); // Go to sleep until signal is received.
asobhy 10:8919b1b76243 85
asobhy 10:8919b1b76243 86 blocksR = pixyR.getBlocks();
asobhy 10:8919b1b76243 87 if (blocksR) {
asobhy 10:8919b1b76243 88 // signature 0 is cloudy day
asobhy 10:8919b1b76243 89 // signature 1 is indoor lighting
asobhy 10:8919b1b76243 90 xR = pixyR.blocks[SIGNATURE_IN_USE].x;
asobhy 10:8919b1b76243 91 yR = pixyR.blocks[SIGNATURE_IN_USE].y;
asobhy 10:8919b1b76243 92 ObjectWidth = pixyR.blocks[SIGNATURE_IN_USE].width;
asobhy 10:8919b1b76243 93 ObjectHeight = pixyR.blocks[SIGNATURE_IN_USE].height;
asobhy 10:8919b1b76243 94 ObjectArea = ObjectHeight * ObjectWidth;
asobhy 10:8919b1b76243 95
asobhy 10:8919b1b76243 96 DistanceError = DISTANCE - ObjectWidth;
asobhy 10:8919b1b76243 97 SteeringError = CENTER - xR;
asobhy 10:8919b1b76243 98
asobhy 10:8919b1b76243 99 }
asobhy 10:8919b1b76243 100
asobhy 10:8919b1b76243 101 }
asobhy 10:8919b1b76243 102
asobhy 10:8919b1b76243 103 }
asobhy 10:8919b1b76243 104
asobhy 10:8919b1b76243 105 /*******************************************************************************
asobhy 10:8919b1b76243 106 * @brief The ISR below signals the CameraThread. it is setup to run
asobhy 10:8919b1b76243 107 * every 20ms
asobhy 10:8919b1b76243 108 * @param none
asobhy 10:8919b1b76243 109 * @return none
asobhy 10:8919b1b76243 110 *******************************************************************************/
asobhy 10:8919b1b76243 111 void CameraPeriodicInterruptISR(void)
asobhy 10:8919b1b76243 112 {
asobhy 10:8919b1b76243 113 // Send signal to the thread with ID, PeriodicInterruptId, i.e., PeriodicInterruptThread.
asobhy 10:8919b1b76243 114 osSignalSet(CameraId,0x1);
asobhy 10:8919b1b76243 115 }
asobhy 10:8919b1b76243 116