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);
     
 }
+
+