ManualControl

Dependencies:   TPixy-Interface

Fork of MbedOS_Robot_Team by ECE4333 - 2018 - Ahmed & Brandon

Files at this revision

API Documentation at this revision

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

CameraThread.cpp Show annotated file Show diff for this revision Revisions of this file
PiControlThread.cpp Show annotated file Show diff for this revision Revisions of this file
ui.cpp Show annotated file Show diff for this revision Revisions of this file
--- 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);
     
 }
+
+