Diff: PiControlThread.cpp

Revision:

23:1839085ffdcf

Parent:

22:c09acff62e6a

--- a/PiControlThread.cpp	Fri Mar 23 22:42:41 2018 +0000
+++ b/PiControlThread.cpp	Thu Mar 29 22:33:52 2018 +0000
@@ -23,13 +23,24 @@
 #include "ui.h"
 #include "CameraThread.h"
 
+
+
 // setpoints
 // speed
 int setpointR = 0;
 int setpointL = 0;
 
+dp_t dpArray;
 
-// 
+int iEnd;
+
+bool memoryFull = false;
+bool patrolStart = true;
+bool reverse = true;
+bool do180 = true;
+int cnt=0;
+
+//
 int velR, velL;
 
 // control signal
@@ -62,13 +73,13 @@
 /******************************************************************************/
 
 // Declare PeriodicInterruptThread as a thread/process
-osThreadDef(PiControlThread, osPriorityRealtime, 1024); 
+osThreadDef(PiControlThread, osPriorityRealtime, 1024);
 
 Ticker PeriodicInt;      // Declare a timer interrupt: PeriodicInt
 
 /*******************************************************************************
-* @brief    function that creates a thread for the PI controller. It initializes 
-*           the PI controller's gains and initializes the DC Motor. It also 
+* @brief    function that creates a thread for the PI controller. It initializes
+*           the PI controller's gains and initializes the DC Motor. It also
 *           initializes the PIControllerThread runs at 50ms period
 * @param    none
 * @return   none
@@ -80,82 +91,151 @@
     motorDriver_L_init();        // initialize motorDriver
     //                Kp,Ki
     PiController_init(1,0.4); // initialize the PI Controller gains and initialize variables
-    
+
     PiControlId = osThreadCreate(osThread(PiControlThread), NULL);
 
     // Specify address of the PeriodicInt ISR as PiControllerISR, specify the interval
     // in seconds between interrupts, and start interrupt generation:
     PeriodicInt.attach(&PeriodicInterruptISR, 0.010);   // 10ms sampling period -> 100Hz freq
-     
+
 }
 
 /*******************************************************************************
-* @brief    This is the PI controller thread. It reads several values from the 
+* @brief    This is the PI controller thread. It reads several values from the
 *           FPGA such as speed, time and other sensors data
 * @param    none
 * @return   none
 *******************************************************************************/
 void PiControlThread(void const *argument)
 {
-    
-    while (true) 
-    {
+    dpArray.i = 0;
+    dpArray.size = ARRAY_SIZE;
+
+    while (true) {
+
         osSignalWait(0x01, osWaitForever); // Go to sleep until signal, SignalPi, is received.
-        
+
         // Get incremental position and time from QEI
         DE0_read(&sensors);
-        
+
         // might not be needed
         sensors.dp_right = SaturateValue(sensors.dp_right, 112);
         sensors.dp_left = SaturateValue(sensors.dp_left, 112);
-        
+
         // Maximum angular velocity @ dPostition = 112 is vel = 703 rad/sec
-        velR = (float)((6135.92 * sensors.dp_right) / sensors.dt_right) ;
-        
+        //velR = (float)((6135.92 * sensors.dp_right) / sensors.dt_right) ;
+
         // Maximum angular velocity @ dPostition = 112 is vel = 703 rad/sec
-        velL = (float)((6135.92 * sensors.dp_left) / sensors.dt_left) ;
-        
-        /********************Manual Setpoint and Steering**********************/
-        mutexSetpoint.lock();
-        setpointR = Setpoint + (Ks*SteeringError);
-        setpointL = Setpoint - (Ks*SteeringError);
-        mutexSetpoint.unlock();    
+        //velL = (float)((6135.92 * sensors.dp_left) / sensors.dt_left) ;
+
+        // if manual control is ON use Differential drive
+        if(MC) {
+            /******************Manual Setpoint and Steering********************/
+            mutexSetpoint.lock();
+            setpointR = Setpoint + (Ks*SteeringError);
+            setpointL = Setpoint - (Ks*SteeringError);
+            mutexSetpoint.unlock();
+            /******************Differential End********************************/
+
+            // Make sure our limit is not exceeded
+            setpointR = SaturateValue(setpointR, 112);
+            setpointL = SaturateValue(setpointL, 112);
+
+            /**********************Record Start********************************/
+            // Record dp if there is room in memory
+            if( (dpArray.i < dpArray.size) ) {
+
+                // only record data when record button is pressed
+                if(startRecording) {
+                    dpArray.dpR[dpArray.i] = setpointR;
+                    dpArray.dpL[dpArray.i] = setpointL;
+                    dpArray.i++;
+                    patrolStart = true;
+                }
+
+            } else {
+                // memory is full
+                memoryFull = true;
+            }
+            /************************Record End********************************/
             
-        // Make sure our limit is not exceeded
-        setpointR = SaturateValue(setpointR, 112);
-        setpointL = SaturateValue(setpointL, 112);
-       
-        /*********************Differential End*********************************/
+            /*********************Playback Start*******************************/
+        } else { // if manual control is not ON then Playback data
+
+            // Check if its the first time to enter the patrol mode
+            if(patrolStart) {
+                // Store end index value
+                iEnd = dpArray.i;
+                // Reset playback index
+                dpArray.i = 0;
+                patrolStart = false;
+            }
+
+            // if do the 180 degree is true then do a 180 rotation
+            if( do180 == true ) {
+                // distance = 10ms * cnt * speed
+                if ( cnt < 300 ) {
+                    setpointR = -10;
+                    setpointL = 10;
+                    cnt++;
+                }
+                // exit the 180 code when done
+                else {
+                    do180 = false;
+                    cnt = 0;
+                }
+            }
+            else if ( dpArray.i < iEnd ) {
+                
+                // Data Playback normal direction
+                if(!reverse) {
+                    setpointR = dpArray.dpR[dpArray.i];
+                    setpointL = dpArray.dpL[dpArray.i];
+                }
+                // Data Playback in reverse direction
+                else {
+                // Wheel dps are swaped to mantain correct direction playback
+                    setpointR = dpArray.dpL[dpArray.i];
+                    setpointL = dpArray.dpR[dpArray.i];
+                }
+                dpArray.i++;
+
+            } else {
+                // We reached the end of our playback we need to toggle reverse
+                reverse = !reverse;
+                // start from the beginning by reseting the index
+                dpArray.i = 0;
+                // Do a 180 degree turn
+                do180 = true;
+            }
+
+
+            /***********************Playback End*******************************/
+        }
 
         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)
-        {
+        if (U_right >= 0) {
             motorDriver_R_forward(U_right);
-        }
-        else if (U_right < 0)
-        {
+        } else if (U_right < 0) {
             motorDriver_R_reverse(U_right);
         }
-        
+
         // Set speed and direction for left motor
-        if (U_left >= 0)
-        {
+        if (U_left >= 0) {
             motorDriver_L_forward(U_left);
-        }
-        else if (U_left < 0)
-        {
+        } else if (U_left < 0) {
             motorDriver_L_reverse(U_left);
         }
-           
+
     }
-    
+
 }
 
 /*******************************************************************************
-* @brief    The ISR below signals the PIControllerThread. it is setup to run 
+* @brief    The ISR below signals the PIControllerThread. it is setup to run
 *           every 10ms
 * @param    none
 * @return   none
@@ -163,6 +243,5 @@
 void PeriodicInterruptISR(void)
 {
     // Send signal to the thread with ID, PeriodicInterruptId, i.e., PeriodicInterruptThread.
-    osSignalSet(PiControlId,0x1); 
+    osSignalSet(PiControlId,0x1);
 }
-