Diff: PiControlThread.cpp

Revision:

9:fe56b888985c

Parent:

8:a0890fa79084

Child:

10:8919b1b76243

--- a/PiControlThread.cpp	Mon Feb 19 17:42:33 2018 +0000
+++ b/PiControlThread.cpp	Fri Feb 23 20:58:34 2018 +0000
@@ -1,9 +1,9 @@
 /******************************************************************************/
 // ECE4333
-// LAB Partner 1: Ahmed Sobhy - ID: 3594449
-// LAB Partner 2: Brandon Kingman - ID: 3470444
-// Project: Autonomous Robot Design
-// Instructor: Prof. Chris Diduch
+// LAB Partner 1:   Ahmed Sobhy - ID: 3594449
+// LAB Partner 2:   Brandon Kingman - ID: 3470444
+// Project:         Autonomous Robot Design
+// Instructor:      Prof. Chris Diduch
 /******************************************************************************/
 // filename: PiControlThread.cpp
 // file content description:
@@ -15,17 +15,18 @@
 
 #include "mbed.h"
 #include "ui.h"
-#include "Drivers/motor_driver.h"
+#include "Drivers/motor_driver_r.h"
+#include "Drivers/motor_driver_l.h"
 #include "Drivers/DE0_driver.h"
 #include "PiControlThread.h"
 #include "Drivers/PiController.h"
 
-extern int setpoint;
+extern int setpointR, setpointL;
 
-uint16_t ID, dTime;
-int dPosition;
-int vel;
-int32_t U;
+int velR, velL;
+int32_t U_right, U_left;
+
+sensors_t sensors;
 
 int time_passed = 0;
 
@@ -61,7 +62,8 @@
 void PiControlThreadInit()
 {
     DE0_init();                  // initialize FPGA
-    motorDriver_init();          // initialize motorDriver
+    motorDriver_R_init();          // initialize motorDriver
+    motorDriver_L_init();          // initialize motorDriver
     //                Kp,Ki
     PiController_init(1,0.4); // initialize the PI Controller gains and initialize variables
     
@@ -87,24 +89,42 @@
     {
         osSignalWait(0x01, osWaitForever); // Go to sleep until signal, SignalPi, is received.
         
-        time_passed++;
+        //time_passed++;
         
         // get incremental position and time from QEI
-        DE0_read(&ID, &dPosition, &dTime);
-        SaturateValue(dPosition, 560);
+        DE0_read(&sensors);
+        
+        SaturateValue(sensors.dp_right, 560);
+        SaturateValue(sensors.dp_left, 560);
+        
+        // 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
-        vel = (float)((6135.92 * dPosition) / dTime) ;
+        velL = (float)((6135.92 * sensors.dp_left) / sensors.dt_left) ;
+        
         
-        U = PiController(setpoint,dPosition);
+        U_right = PiControllerR(setpointR,sensors.dp_right);
+        U_left  = PiControllerL(setpointL,sensors.dp_left);
         
-        if (U >= 0)
+        // set speed and direction for right motor
+        if (U_right >= 0)
         {
-            motorDriver_forward(U);
+            motorDriver_R_forward(U_right);
+        }
+        else if (U_right < 0)
+        {
+            motorDriver_R_reverse(U_right);
         }
-        else if (U < 0)
+        
+        // set speed and direction of left motor
+        if (U_left >= 0)
         {
-            motorDriver_reverse(U);
+            motorDriver_L_forward(U_left);
+        }
+        else if (U_left < 0)
+        {
+            motorDriver_L_reverse(U_left);
         }
            
     }