Diff: PiControlThread.cpp

Revision:

17:1184df616383

Parent:

16:58ec2b891a25

Child:

19:4fbffc755ed7

--- a/PiControlThread.cpp	Sun Mar 11 00:37:58 2018 +0000
+++ b/PiControlThread.cpp	Sun Mar 11 01:26:23 2018 +0000
@@ -41,12 +41,12 @@
 
 // steering gain
 float Ks = 0.15;
-
 // distance gain
 float Kd = 10;
 
 // overall robot required speed
 int Setpoint;
+Mutex mutexSetpoint;
 
 osThreadId PiControlId;
 
@@ -125,7 +125,7 @@
         // If distance increase then speed should increase.
         
         // If object is moving away from the the robot increase robot speed
-        cameraData_mutex.lock();
+        /*cameraData_mutex.lock();
         if(DistanceError > 0)
         {
             // Proportional controller
@@ -142,7 +142,14 @@
         setpointR = Setpoint + (Ks*SteeringError);
         setpointL = Setpoint - (Ks*SteeringError);
         cameraData_mutex.unlock();
+        */
         
+        /********************Manual Setpoint and Steering**********************/
+        mutexSetpoint.lock();
+        setpointR = Setpoint + (Ks*SteeringError);
+        setpointL = Setpoint - (Ks*SteeringError);
+        mutexSetpoint.unlock();    
+            
         // Make sure our limit is not exceeded
         setpointR = SaturateValue(setpointR, 560);
         setpointL = SaturateValue(setpointL, 560);
@@ -162,7 +169,7 @@
             motorDriver_R_reverse(U_right);
         }
         
-        // Set speed and direction of left motor
+        // Set speed and direction for left motor
         if (U_left >= 0)
         {
             motorDriver_L_forward(U_left);