Diff: PiControlThread.cpp

Revision:

15:cf67f83d5409

Parent:

14:5777377537a2

Child:

16:58ec2b891a25

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