Diff: PiControlThread.cpp

Revision:

14:5777377537a2

Parent:

11:9135e5bc2fcf

Child:

15:cf67f83d5409

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