Diff: PiControlThread.cpp

Revision:

10:8919b1b76243

Parent:

9:fe56b888985c

Child:

11:9135e5bc2fcf

diff -r fe56b888985c -r 8919b1b76243 PiControlThread.cpp
--- a/PiControlThread.cpp	Fri Feb 23 20:58:34 2018 +0000
+++ b/PiControlThread.cpp	Fri Mar 02 23:37:31 2018 +0000
@@ -21,9 +21,14 @@
 #include "PiControlThread.h"
 #include "Drivers/PiController.h"
 
+// setpoints
 extern int setpointR, setpointL;
+extern int SteeringError, DistanceError;
 
+// 
 int velR, velL;
+
+// control signal
 int32_t U_right, U_left;
 
 sensors_t sensors;
@@ -33,6 +38,15 @@
 void PiControlThread(void const *);
 void PeriodicInterruptISR(void);
 
+// steering gain
+int Ks = 1;
+
+// distance gain
+int Kd = 1;
+
+// overall robot required speed
+int Setpoint;
+
 osThreadId PiControlId;
 
 /******************************************************************************/
@@ -76,6 +90,10 @@
 }
 
 
+
+
+
+
 /*******************************************************************************
 * @brief    This is the PI controller thread. It reads several values from the 
 *           FPGA such as speed, time and other sensors data
@@ -89,8 +107,6 @@
     {
         osSignalWait(0x01, osWaitForever); // Go to sleep until signal, SignalPi, is received.
         
-        //time_passed++;
-        
         // get incremental position and time from QEI
         DE0_read(&sensors);
         
@@ -103,6 +119,27 @@
         // maximum velocity at dPostition = 560 is vel = 703
         velL = (float)((6135.92 * sensors.dp_left) / sensors.dt_left) ;
         
+        /*********************Differential Start*******************************/
+        // Inputs are Speed Setpoint and Steering Setpoint
+        // The Inputs for the Steering are specified in the CameraThread
+        // and set at the center.
+        // The distance between the object and the image should be set to 1 meter   
+        // 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)
+        {
+            Setpoint = (Kd*DistanceError);
+        }
+        // if object is at the set distance limit or less then do not move.
+        else if(DistanceError <= 0)
+        {
+            Setpoint = 0;
+        }
+        
+        setpointR = Setpoint + (Ks*SteeringError);
+        setpointL = Setpoint - (Ks*SteeringError);
         
         U_right = PiControllerR(setpointR,sensors.dp_right);
         U_left  = PiControllerL(setpointL,sensors.dp_left);