ManualControl
Dependencies: TPixy-Interface
Fork of MbedOS_Robot_Team by
Revision 15:cf67f83d5409, committed 2018-03-03
- Comitter:
- asobhy
- Date:
- Sat Mar 03 04:47:26 2018 +0000
- Parent:
- 14:5777377537a2
- Child:
- 16:58ec2b891a25
- Commit message:
- 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.
Changed in this revision
--- a/CameraThread.cpp Sat Mar 03 02:14:40 2018 +0000 +++ b/CameraThread.cpp Sat Mar 03 04:47:26 2018 +0000 @@ -15,15 +15,13 @@ #include "mbed.h" #include "Pixy.h" +#include "ui.h" #define CENTER 160 #define DISTANCE 20 #define SIGNATURE_IN_USE 0 osThreadId CameraId; - -extern Serial bluetooth; - Mutex cameraData_mutex; SPI spiR(p11, p12, p13); // (mosi, miso, sclk) @@ -69,16 +67,9 @@ ObjectArea=0; SteeringError=0; DistanceError=0; - - // Reset all storage - xRAvg=0; - yRAvg=0; - ObjectWidthAvg=0; - ObjectHeightAvg=0; - ObjectAreaAvg=0; CameraId = osThreadCreate(osThread(CameraThread), NULL); - CameraPeriodicInt.attach(&CameraPeriodicInterruptISR, 0.05); // 500ms sampling rate + CameraPeriodicInt.attach(&CameraPeriodicInterruptISR, 0.5); // 500ms sampling rate } @@ -90,7 +81,6 @@ *******************************************************************************/ void CameraThread(void const *argument) { - static int count = 0; while (true) { @@ -101,45 +91,28 @@ if (blocksR) { // signature 0 is cloudy day // signature 1 is indoor lighting - + + cameraData_mutex.lock(); 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; - // Accumulate readings to be used for average value calculation - xRAvg += xR; - yRAvg += yR; - ObjectWidthAvg += ObjectWidth; - ObjectHeightAvg += ObjectHeight; - ObjectAreaAvg += ObjectArea; - count++; - // Average calculation 10 readings - if(count > 10) - { - xRAvg=xRAvg/10; - yRAvg=yRAvg/10; - ObjectWidthAvg=ObjectWidthAvg/10; - ObjectHeightAvg=ObjectHeightAvg/10; - ObjectAreaAvg=ObjectAreaAvg/10; - - cameraData_mutex.lock(); - DistanceError = DISTANCE - ObjectWidthAvg; - SteeringError = CENTER - xRAvg; - cameraData_mutex.unlock(); - - count = 0; - // Reset all storage - xRAvg=0; - yRAvg=0; - ObjectWidthAvg=0; - ObjectHeightAvg=0; - ObjectAreaAvg=0; - } + DistanceError = DISTANCE - ObjectWidth; + SteeringError = CENTER - xR; + cameraData_mutex.unlock(); } + // ball not found stop robot from moving + else + { + // set Distance error = 0 + DistanceError = 0; + SteeringError = 0; + } + }
--- a/CameraThread.h Sat Mar 03 02:14:40 2018 +0000 +++ b/CameraThread.h Sat Mar 03 04:47:26 2018 +0000 @@ -10,6 +10,12 @@ #ifndef CAMERA_INT_H #define CAMERA_INT_H +extern int SteeringError, DistanceError; +extern Mutex cameraData_mutex; +extern int xR, yR, ObjectWidth, ObjectHeight, ObjectArea, SteeringError, DistanceError; +extern int xRAvg, yRAvg, ObjectWidthAvg, ObjectHeightAvg, ObjectAreaAvg; + + void CameraThreadInit(void);
--- a/Drivers/PiController.cpp Sat Mar 03 02:14:40 2018 +0000 +++ b/Drivers/PiController.cpp Sat Mar 03 04:47:26 2018 +0000 @@ -14,10 +14,9 @@ #include "mbed.h" #include "PiController.h" +#include "ui.h" // global speed variable; -extern Mutex setpointR_mutex; -extern Mutex setpointL_mutex; float Ki; float Kp; int32_t scale;
--- a/PiControlThread.cpp Sat Mar 03 02:14:40 2018 +0000 +++ b/PiControlThread.cpp Sat Mar 03 04:47:26 2018 +0000 @@ -20,10 +20,11 @@ #include "Drivers/DE0_driver.h" #include "PiControlThread.h" #include "Drivers/PiController.h" +#include "ui.h" +#include "CameraThread.h" // setpoints -extern int setpointR, setpointL; -extern int SteeringError, DistanceError; + // int velR, velL; @@ -39,10 +40,10 @@ void PeriodicInterruptISR(void); // steering gain -int Ks = 2; +int Ks = 0.5; // distance gain -int Kd = 2; +int Kd = 4; // overall robot required speed int Setpoint; @@ -105,8 +106,9 @@ // Get incremental position and time from QEI DE0_read(&sensors); - SaturateValue(sensors.dp_right, 560); - SaturateValue(sensors.dp_left, 560); + // might not be needed + sensors.dp_right = SaturateValue(sensors.dp_right, 560); + sensors.dp_left = SaturateValue(sensors.dp_left, 560); // Maximum velocity at dPostition = 560 is vel = 703 velR = (float)((6135.92 * sensors.dp_right) / sensors.dt_right) ; @@ -123,10 +125,12 @@ // If distance increase then speed should increase. // If object is moving away from the the robot increase robot speed + cameraData_mutex.lock(); if(DistanceError > 0) { // Proportional controller Setpoint = (Kd*DistanceError); + Setpoint = SaturateValue(Setpoint,560); } // If object is at the set distance limit or less then do not move. else if(DistanceError <= 0) @@ -137,6 +141,7 @@ // Decoupled drive setpointR = Setpoint + (Ks*SteeringError); setpointL = Setpoint - (Ks*SteeringError); + cameraData_mutex.unlock(); // Make sure our limit is not exceeded setpointR = SaturateValue(setpointR, 560);
--- a/PiControlThread.h Sat Mar 03 02:14:40 2018 +0000 +++ b/PiControlThread.h Sat Mar 03 04:47:26 2018 +0000 @@ -9,6 +9,7 @@ #ifndef PERIODIC_INT_H #define PERIODIC_INT_H +extern osThreadId PiControlId; void PiControlThreadInit(void);
--- a/main.cpp Sat Mar 03 02:14:40 2018 +0000 +++ b/main.cpp Sat Mar 03 04:47:26 2018 +0000 @@ -12,6 +12,8 @@ #include "ExternalInterruptThread.h" #include "ui.h" #include "CameraThread.h" +#include "motor_driver_l.h" +#include "motor_driver_r.h" /******************************************************************************/ @@ -34,6 +36,14 @@ while(1) { + // if robot goes crazy press 'r' to kill it + if(killRobot == true) + { + osThreadTerminate(PiControlId); + motorDriver_L_stop(); + motorDriver_R_stop(); + } + consoleUI(); Thread::wait(500); // Go to sleep for 500 ms }
--- a/ui.cpp Sat Mar 03 02:14:40 2018 +0000 +++ b/ui.cpp Sat Mar 03 04:47:26 2018 +0000 @@ -13,6 +13,7 @@ #include "mbed.h" #include "WatchdogThread.h" #include "ui.h" +#include "CameraThread.h" Serial bluetooth(p9,p10); // Define the bluetooth channel and IO pins Serial pc(USBTX, USBRX); // Pins (tx, rx) for PC serial channel @@ -24,23 +25,11 @@ int setpointR = 0; int setpointL = 0; +bool killRobot = false; + // variable to store character recieved from terminal char x; -/* -extern int16_t dPosition, dTime; -extern float Ki; -extern float Kp; -extern int vel; -extern int32_t e; -extern int32_t xState; -extern int32_t u; -//extern int time_passed; -*/ - -extern int xR, yR, ObjectWidth, ObjectHeight, ObjectArea, SteeringError, DistanceError; -extern int xRAvg, yRAvg, ObjectWidthAvg, ObjectHeightAvg, ObjectAreaAvg; -extern Mutex cameraData_mutex; int16_t position; /****************************************************************************** @@ -48,8 +37,24 @@ ******************************************************************************/ void consoleUI(void){ - //bluetooth.printf("\r\nsetpointR: %d setpointL: %d ObjectWidth: %d ObjectXcoordinate: %d SteeringError: %d DistanceError: %d", setpointR, setpointL, ObjectWidth, xR, SteeringError, DistanceError); - bluetooth.printf("\r\nsetpointR: %d setpointL: %d ObjectWidth: %d ObjectXcoordinate: %d SteeringError: %d DistanceError: %d", setpointR, setpointL, ObjectWidth, xR, SteeringError, DistanceError); + cameraData_mutex.lock(); + bluetooth.printf("\r\nsetpointR: %d setpointL: %d ObjectWidth: %d ObjectXcoordinate: %d SteeringError: %d DistanceError: %d", setpointR, setpointL, ObjectWidth, xR, SteeringError, DistanceError); + cameraData_mutex.unlock(); + //bluetooth.printf("\r\nsetpointR: %d setpointL: %d ObjectWidth: %d ObjectXcoordinate: %d SteeringError: %d DistanceError: %d", setpointR, setpointL, ObjectWidth, xR, SteeringError, DistanceError); + + // safety mechanism + if (bluetooth.readable()){ + x = bluetooth.getc(); + if (x == 'r') + { + killRobot = true; + } + else if (x == 's') + { + killRobot = false; + } + } + } @@ -75,7 +80,7 @@ /****************************************************************************** A function to Display startup Messsage ******************************************************************************/ -void displayStartupMsg() +void displayStartupMsg_old() { bluetooth.printf("\r\n************************************"); bluetooth.printf("\r\n**** DC Motor Control using PWM ****"); @@ -87,6 +92,13 @@ bluetooth.printf("\r\n-press k to decrease motor speedL"); } +void displayStartupMsg() +{ + bluetooth.printf("\r\n************************************"); + bluetooth.printf("\r\n**** AUTONOMOUS FOLLOWING ROBOT ****"); + bluetooth.printf("\r\n************************************"); + bluetooth.printf("\r\n-PRESS 'r' TO KILL ROBOT"); +} /****************************************************************************** User interface 1
--- a/ui.h Sat Mar 03 02:14:40 2018 +0000 +++ b/ui.h Sat Mar 03 04:47:26 2018 +0000 @@ -11,6 +11,13 @@ #define SPEED_STEP 1 +extern bool killRobot; +extern int setpointR; +extern int setpointL; +extern Serial bluetooth; +extern Mutex setpointR_mutex; +extern Mutex setpointL_mutex; + void consoleUI(void); void displayStartupMsg(void); void twoTerminalsTest(void);