Diff: PiControlThread.cpp

Revision:

25:f2a1bd6591fb

Parent:

24:e88753f090b8

Child:

27:510b4af89a6b

--- a/PiControlThread.cpp	Tue Apr 03 16:00:10 2018 +0000
+++ b/PiControlThread.cpp	Tue Apr 03 16:39:12 2018 +0000
@@ -40,10 +40,10 @@
 void PeriodicInterruptISR(void);
 
 // steering gain
-float Ks = 0.15;
+float Ks = 0.1;
 
 // distance gain
-float Kd = 10;
+float Kd = 5;
 
 // overall robot required speed
 int Setpoint;
@@ -139,8 +139,8 @@
         }
         
         // Decoupled drive  
-        setpointR = Setpoint + (Ks*SteeringError);
-        setpointL = Setpoint - (Ks*SteeringError);
+        setpointL = Setpoint + (Ks*SteeringError);
+        setpointR = Setpoint - (Ks*SteeringError);
         cameraData_mutex.unlock();
         
         // Make sure our limit is not exceeded
@@ -149,8 +149,8 @@
        
         /*********************Differential End*********************************/
 
-        U_right = PiControllerR(setpointR,sensors.dp_right);
-        U_left  = PiControllerL(setpointL,sensors.dp_left);
+        U_right = PiControllerR(setpointR,-sensors.dp_right);
+        U_left  = PiControllerL(setpointL,-sensors.dp_left);
         
         // Set speed and direction for right motor
         if (U_right >= 0)