ManualControl
Dependencies: TPixy-Interface
Fork of MbedOS_Robot by
Revision 14:5777377537a2, committed 2018-03-03
- Comitter:
- asobhy
- Date:
- Sat Mar 03 02:14:40 2018 +0000
- Parent:
- 13:2a00b7a6f4bd
- Child:
- 15:cf67f83d5409
- Commit message:
- averaging code added to following robot but results are not that good
Changed in this revision
--- a/CameraThread.cpp Sat Mar 03 00:53:06 2018 +0000 +++ b/CameraThread.cpp Sat Mar 03 02:14:40 2018 +0000 @@ -24,6 +24,8 @@ extern Serial bluetooth; +Mutex cameraData_mutex; + SPI spiR(p11, p12, p13); // (mosi, miso, sclk) PixySPI pixyR(&spiR, &bluetooth); @@ -46,6 +48,8 @@ Ticker CameraPeriodicInt; // Declare a timer interrupt: PeriodicInt int xR, yR, ObjectWidth, ObjectHeight, ObjectArea, SteeringError, DistanceError; +int xRAvg, yRAvg, ObjectWidthAvg, ObjectHeightAvg, ObjectAreaAvg; + uint16_t blocksR; /******************************************************************************* @@ -56,6 +60,8 @@ void CameraThreadInit() { pixyR.init(); + + // Reset all storage xR=0; yR=0; ObjectWidth=0; @@ -64,8 +70,15 @@ 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.5); // 500ms sampling rate + CameraPeriodicInt.attach(&CameraPeriodicInterruptISR, 0.05); // 500ms sampling rate } @@ -77,6 +90,7 @@ *******************************************************************************/ void CameraThread(void const *argument) { + static int count = 0; while (true) { @@ -87,14 +101,43 @@ 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; + // 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; + } }
--- a/PiControlThread.cpp Sat Mar 03 00:53:06 2018 +0000 +++ b/PiControlThread.cpp Sat Mar 03 02:14:40 2018 +0000 @@ -39,10 +39,10 @@ void PeriodicInterruptISR(void); // steering gain -int Ks = 1; +int Ks = 2; // distance gain -int Kd = 1; +int Kd = 2; // overall robot required speed int Setpoint; @@ -102,16 +102,16 @@ { osSignalWait(0x01, osWaitForever); // Go to sleep until signal, SignalPi, is received. - // get incremental position and time from QEI + // Get incremental position and time from QEI DE0_read(&sensors); SaturateValue(sensors.dp_right, 560); SaturateValue(sensors.dp_left, 560); - // maximum velocity at dPostition = 560 is vel = 703 + // Maximum velocity at dPostition = 560 is vel = 703 velR = (float)((6135.92 * sensors.dp_right) / sensors.dt_right) ; - // maximum velocity at dPostition = 560 is vel = 703 + // Maximum velocity at dPostition = 560 is vel = 703 velL = (float)((6135.92 * sensors.dp_left) / sensors.dt_left) ; /*********************Differential Start*******************************/ @@ -122,24 +122,32 @@ // 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) + // If object is moving away from the the robot increase robot speed + if(DistanceError > 0) { + // Proportional controller Setpoint = (Kd*DistanceError); } - // if object is at the set distance limit or less then do not move. + // If object is at the set distance limit or less then do not move. else if(DistanceError <= 0) { Setpoint = 0; } + // Decoupled drive setpointR = Setpoint + (Ks*SteeringError); setpointL = Setpoint - (Ks*SteeringError); - */ + + // Make sure our limit is not exceeded + setpointR = SaturateValue(setpointR, 560); + setpointL = SaturateValue(setpointL, 560); + + /*********************Differential End*********************************/ + U_right = PiControllerR(setpointR,sensors.dp_right); U_left = PiControllerL(setpointL,sensors.dp_left); - // set speed and direction for right motor + // Set speed and direction for right motor if (U_right >= 0) { motorDriver_R_forward(U_right); @@ -149,7 +157,7 @@ motorDriver_R_reverse(U_right); } - // set speed and direction of left motor + // Set speed and direction of left motor if (U_left >= 0) { motorDriver_L_forward(U_left);
--- a/ui.cpp Sat Mar 03 00:53:06 2018 +0000 +++ b/ui.cpp Sat Mar 03 02:14:40 2018 +0000 @@ -38,8 +38,20 @@ //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; +/****************************************************************************** + User interface 3 +******************************************************************************/ +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); +} + /****************************************************************************** @@ -138,7 +150,7 @@ User interface 2 ******************************************************************************/ -void consoleUI(void) +void consoleUIold(void) { if (bluetooth.readable()) { @@ -234,3 +246,5 @@ //bluetooth.printf("\r\ne: %d, Pos: %d, dP: %d, xState: %d, u: %d, dT: %d", e, position, dPosition, xState, u, dTime); } + +