ManualControl
Dependencies: TPixy-Interface
Fork of MbedOS_Robot by
Revision 10:8919b1b76243, committed 2018-03-02
- Comitter:
- asobhy
- Date:
- Fri Mar 02 23:37:31 2018 +0000
- Parent:
- 9:fe56b888985c
- Child:
- 11:9135e5bc2fcf
- Commit message:
- vision + robot testing
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/CameraThread.cpp Fri Mar 02 23:37:31 2018 +0000 @@ -0,0 +1,116 @@ +/******************************************************************************/ +// ECE4333 +// LAB Partner 1: Ahmed Sobhy - ID: 3594449 +// LAB Partner 2: Brandon Kingman - ID: 3470444 +// Project: Autonomous Robot Design +// Instructor: Prof. Chris Diduch +/******************************************************************************/ +// filename: CameraThread.cpp +// file content description: +// * Function to Create the Camera Thread +// * CameraThread +// * CameraThread ISR +// * Variables related to the functionality of the thread +/******************************************************************************/ + +#include "mbed.h" +#include "Pixy.h" + +#define CENTER 160 +#define DISTANCE 20 +#define SIGNATURE_IN_USE 0 + +osThreadId CameraId; + +extern Serial bluetooth; + +SPI spiR(p11, p12, p13); // (mosi, miso, sclk) +PixySPI pixyR(&spiR, &bluetooth); + +void CameraThread(void const *argument); +void CameraPeriodicInterruptISR(void); + +/******************************************************************************/ +// osPriorityIdle = -3, ///< priority: idle (lowest) +// osPriorityLow = -2, ///< priority: low +// osPriorityBelowNormal = -1, ///< priority: below normal +// osPriorityNormal = 0, ///< priority: normal (default) +// osPriorityAboveNormal = +1, ///< priority: above normal +// osPriorityHigh = +2, ///< priority: high +// osPriorityRealtime = +3, ///< priority: realtime (highest) +/******************************************************************************/ + +// Declare PeriodicInterruptThread as a thread/process +osThreadDef(CameraThread, osPriorityHigh, 1024); + +Ticker CameraPeriodicInt; // Declare a timer interrupt: PeriodicInt + +int xR, yR, ObjectWidth, ObjectHeight, ObjectArea, SteeringError, DistanceError; +uint16_t blocksR; + +/******************************************************************************* +* @brief function that creates a thread for the Camera +* @param none +* @return none +*******************************************************************************/ +void CameraThreadInit() +{ + pixyR.init(); + xR=0; + yR=0; + ObjectWidth=0; + ObjectHeight=0; + ObjectArea=0; + SteeringError=0; + DistanceError=0; + + CameraId = osThreadCreate(osThread(CameraThread), NULL); + CameraPeriodicInt.attach(&CameraPeriodicInterruptISR, 0.02); // 20ms sampling rate - 50fps +} + + +/******************************************************************************* +* @brief This is the Camera thread. It reads several values from the +* Camera: x coordinate error from center @ 160 & the block area of the object +* @param none +* @return none +*******************************************************************************/ +void CameraThread(void const *argument) +{ + + while (true) + { + + osSignalWait(0x01, osWaitForever); // Go to sleep until signal is received. + + blocksR = pixyR.getBlocks(); + if (blocksR) { + // signature 0 is cloudy day + // signature 1 is indoor lighting + xR = pixyR.blocks[SIGNATURE_IN_USE].x; + yR = pixyR.blocks[SIGNATURE_IN_USE].y; + ObjectWidth = pixyR.blocks[SIGNATURE_IN_USE].width; + ObjectHeight = pixyR.blocks[SIGNATURE_IN_USE].height; + ObjectArea = ObjectHeight * ObjectWidth; + + DistanceError = DISTANCE - ObjectWidth; + SteeringError = CENTER - xR; + + } + + } + +} + +/******************************************************************************* +* @brief The ISR below signals the CameraThread. it is setup to run +* every 20ms +* @param none +* @return none +*******************************************************************************/ +void CameraPeriodicInterruptISR(void) +{ + // Send signal to the thread with ID, PeriodicInterruptId, i.e., PeriodicInterruptThread. + osSignalSet(CameraId,0x1); +} +
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/CameraThread.h Fri Mar 02 23:37:31 2018 +0000 @@ -0,0 +1,18 @@ + +/******************************************************************************/ +// ECE4333 +// LAB Partner 1: Ahmed Sobhy - ID: 3594449 +// LAB Partner 2: Brandon Kingman - ID: 3470444 +// Project: Autonomous Robot Design +// Instructor: Prof. Chris Diduch +/******************************************************************************/ + +#ifndef CAMERA_INT_H +#define CAMERA_INT_H + +void CameraThreadInit(void); + + +#endif + +
--- a/PiControlThread.cpp Fri Feb 23 20:58:34 2018 +0000 +++ b/PiControlThread.cpp Fri Mar 02 23:37:31 2018 +0000 @@ -21,9 +21,14 @@ #include "PiControlThread.h" #include "Drivers/PiController.h" +// setpoints extern int setpointR, setpointL; +extern int SteeringError, DistanceError; +// int velR, velL; + +// control signal int32_t U_right, U_left; sensors_t sensors; @@ -33,6 +38,15 @@ void PiControlThread(void const *); void PeriodicInterruptISR(void); +// steering gain +int Ks = 1; + +// distance gain +int Kd = 1; + +// overall robot required speed +int Setpoint; + osThreadId PiControlId; /******************************************************************************/ @@ -76,6 +90,10 @@ } + + + + /******************************************************************************* * @brief This is the PI controller thread. It reads several values from the * FPGA such as speed, time and other sensors data @@ -89,8 +107,6 @@ { osSignalWait(0x01, osWaitForever); // Go to sleep until signal, SignalPi, is received. - //time_passed++; - // get incremental position and time from QEI DE0_read(&sensors); @@ -103,6 +119,27 @@ // maximum velocity at dPostition = 560 is vel = 703 velL = (float)((6135.92 * sensors.dp_left) / sensors.dt_left) ; + /*********************Differential Start*******************************/ + // Inputs are Speed Setpoint and Steering Setpoint + // The Inputs for the Steering are specified in the CameraThread + // and set at the center. + // The distance between the object and the image should be set to 1 meter + // If distance decrease then speed should stop. + // If distance increase then speed should increase. + + // if object is moving away from the the robot increase robot speed + if(DistanceError > 0) + { + Setpoint = (Kd*DistanceError); + } + // if object is at the set distance limit or less then do not move. + else if(DistanceError <= 0) + { + Setpoint = 0; + } + + setpointR = Setpoint + (Ks*SteeringError); + setpointL = Setpoint - (Ks*SteeringError); U_right = PiControllerR(setpointR,sensors.dp_right); U_left = PiControllerL(setpointL,sensors.dp_left);
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/TPixy-Interface.lib Fri Mar 02 23:37:31 2018 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/swilkins8/code/TPixy-Interface/#66df7d295245
--- a/main.cpp Fri Feb 23 20:58:34 2018 +0000 +++ b/main.cpp Fri Mar 02 23:37:31 2018 +0000 @@ -11,6 +11,7 @@ #include "PiControlThread.h" #include "ExternalInterruptThread.h" #include "ui.h" +#include "CameraThread.h" /******************************************************************************/ @@ -27,12 +28,13 @@ // Initialize and run the threads below: WatchdogThreadInit(); + CameraThreadInit(); PiControlThreadInit(); ExternalInterruptThreadInit(); while(1) { - consoleUI(); + //consoleUI(); Thread::wait(500); // Go to sleep for 500 ms } }