ManualControl

Dependencies:   TPixy-Interface

Fork of MbedOS_Robot_Team by ECE4333 - 2018 - Ahmed & Brandon

Files at this revision

API Documentation at this revision

Comitter:
asobhy
Date:
Fri Feb 09 18:37:11 2018 +0000
Parent:
3:4def4ca68910
Child:
5:b29220d29022
Child:
6:e7ce340fe91e
Commit message:
beginning of lab 02/09/2018

Changed in this revision

PiControlThread.cpp Show annotated file Show diff for this revision Revisions of this file
PiControlThread.h Show annotated file Show diff for this revision Revisions of this file
PiController.cpp Show annotated file Show diff for this revision Revisions of this file
PiController.h Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
ui.cpp Show annotated file Show diff for this revision Revisions of this file
ui.h Show annotated file Show diff for this revision Revisions of this file
--- a/PiControlThread.cpp	Sat Feb 03 00:48:47 2018 +0000
+++ b/PiControlThread.cpp	Fri Feb 09 18:37:11 2018 +0000
@@ -3,29 +3,17 @@
 #include "Drivers/motor_driver.h"
 #include "Drivers/DE0_driver.h"
 #include "PiControlThread.h"
+#include "PiController.h"
 
-// global speed variable;
 extern int setpoint;
-extern Serial pc;
-extern Mutex setpoint_mutex;
 
 uint16_t ID, dTime;
 int dPosition;
 int vel;
-
-int32_t e, u, xState;
-
-bool saturationFlag;
-
-float Ki; 
-float Kp;
+int32_t U;
 
 void PiControlThread(void const *);
 void PeriodicInterruptISR(void);
-int SaturatingSubtract(int , int );
-int SaturatingAdd(int , int );
-int SaturateValue(int , int );
-
 
 osThreadId PiControlId;
 
@@ -49,18 +37,16 @@
 
 void PiControlThreadInit()
 {
-    DE0_init();             // initialize FPGA
-    motorDriver_init();     // initialize motorDriver
+    DE0_init();                  // initialize FPGA
+    motorDriver_init();          // initialize motorDriver
+    PiController_init(2.2,0.01); // 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.05);   // 50ms sampling rate
-    
-    Kp = 2.2;
-    Ki = 0.01;
-    
+     
 }
 
 
@@ -70,16 +56,6 @@
 void PiControlThread(void const *argument)
 {
     
-    // initialization
-    saturationFlag = false;
-    int scale = 40;
-    xState = 0;  
-    
-    int32_t xTemp;
-    int32_t uProportional;
-    int32_t uIntegral;
-    int32_t uS;
-      
     while (true) 
     {
         osSignalWait(0x01, osWaitForever); // Go to sleep until signal, SignalPi, is received.
@@ -91,30 +67,15 @@
         // maximum velocity at dPostition = 560 is vel = 703
         vel = (float)((6135.92 * dPosition) / dTime) ;
         
-        setpoint_mutex.lock();
-        e = SaturatingSubtract(setpoint, dPosition);  // e is the velocity error
-        setpoint_mutex.unlock();
-            
-        xTemp = SaturatingAdd(xState, e);
-        
-        // the maximum value that 'u' can get to is 20
-        // the maximum value that dPosition can get to 560
-        // scaling factor is 560/20 = 28
-        uProportional = (float)(Kp*e/scale);
-        uIntegral = (float)(Ki*xState/scale);
+        U = PiController(setpoint,dPosition);
         
-        uS = SaturatingAdd(uProportional, uIntegral);
-        
-        u = SaturateValue(uS, U_LIMIT);      
-        if(u==uS) xState=xTemp;
-        
-        if (u >= 0)
+        if (U >= 0)
         {
-            motorDriver_forward(u);
+            motorDriver_forward(U);
         }
-        else if (u < 0)
+        else if (U < 0)
         {
-            motorDriver_reverse(u);
+            motorDriver_reverse(U);
         }   
            
     }
@@ -132,31 +93,3 @@
     osSignalSet(PiControlId,0x1); 
 }
 
-
-/*****************************************************************************/
-int SaturatingSubtract(int x, int y)
-{
-    int z;
-    z = x - y; // 32-bit overflow detection and saturating arithmetic
-    if((x > 0) && (y < 0) && (z < 0)) z = 0x7FFFFFFF;
-    else if((x < 0) && (y > 0) && (z > 0)) z = 0x80000000;
-    return z;
-}
-
-/*****************************************************************************/
-int SaturatingAdd(int x, int y)
-{
-    int z;
-    z = x + y; // 32-bit overflow detection and saturating arithmetic
-    if((x > 0) && (y > 0) && (z < 0)) z = 0x7FFFFFFF;
-    else if((x < 0) && (y < 0) && (z > 0)) z = 0x80000000;
-    return z;
-}
-
-/*****************************************************************************/
-int SaturateValue(int x, int Limit)
-{
-    if(x > Limit) return(Limit); // Impose maximum limit on x
-    else if(x < -Limit) return(-Limit);
-    else return(x);
-}
--- a/PiControlThread.h	Sat Feb 03 00:48:47 2018 +0000
+++ b/PiControlThread.h	Fri Feb 09 18:37:11 2018 +0000
@@ -2,7 +2,6 @@
 #ifndef PERIODIC_INT_H
 #define PERIODIC_INT_H
 
-#define U_LIMIT     20
 
 void PiControlThreadInit(void);
 
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PiController.cpp	Fri Feb 09 18:37:11 2018 +0000
@@ -0,0 +1,86 @@
+#include "mbed.h"
+#include "PiController.h"
+
+
+
+// global speed variable;
+
+extern Mutex setpoint_mutex;
+
+float Ki;
+float Kp;
+int32_t e, u, xState, scale;
+
+
+/*****************************************************************************/
+int SaturatingSubtract(int x, int y)
+{
+    int z;
+    z = x - y; // 32-bit overflow detection and saturating arithmetic
+    if((x > 0) && (y < 0) && (z < 0)) z = 0x7FFFFFFF;
+    else if((x < 0) && (y > 0) && (z > 0)) z = 0x80000000;
+    return z;
+}
+
+/*****************************************************************************/
+int SaturatingAdd(int x, int y)
+{
+    int z;
+    z = x + y; // 32-bit overflow detection and saturating arithmetic
+    if((x > 0) && (y > 0) && (z < 0)) z = 0x7FFFFFFF;
+    else if((x < 0) && (y < 0) && (z > 0)) z = 0x80000000;
+    return z;
+}
+
+/*****************************************************************************/
+int SaturateValue(int x, int Limit)
+{
+    if(x > Limit) return(Limit); // Impose maximum limit on x
+    else if(x < -Limit) return(-Limit);
+    else return(x);
+}
+
+
+/*****************************************************************************/
+void PiController_init(float Kp_given, float Ki_given)
+{
+    Kp = Kp_given;
+    Ki = Ki_given;
+    
+    // initialization
+    scale = 40;
+    xState = 0;  
+        
+}
+
+
+/*****************************************************************************/
+uint32_t PiController(int setp, int dP)
+{
+    
+    int32_t xTemp;
+    int32_t uProportional;
+    int32_t uIntegral;
+    int32_t uS;
+    
+    setpoint_mutex.lock();
+    e = SaturatingSubtract(setp, dP);  // e is the velocity error
+    setpoint_mutex.unlock();
+
+    xTemp = SaturatingAdd(xState, e);
+
+    // the maximum value that 'u' can get to is 20
+    // the maximum value that dPosition can get to 560
+    // scaling factor is 560/20 = 28
+    
+    uProportional = (float)(Kp*e/scale);
+    uIntegral = (float)(Ki*xState/scale);
+
+    uS = SaturatingAdd(uProportional, uIntegral);
+
+    u = SaturateValue(uS, U_LIMIT);
+    if(u==uS) xState=xTemp; // if limit has not been reached then update xState
+    
+    return u;
+    
+}
\ No newline at end of file
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PiController.h	Fri Feb 09 18:37:11 2018 +0000
@@ -0,0 +1,13 @@
+#ifndef PICONTROLLER_H
+#define PICONTROLLER_H
+
+#define U_LIMIT     20
+
+int SaturateValue(int , int );
+int SaturatingSubtract(int x, int y);
+int SaturatingAdd(int x, int y);
+void PiController_init(float,float);
+uint32_t PiController(int,int);
+
+
+#endif
\ No newline at end of file
--- a/main.cpp	Sat Feb 03 00:48:47 2018 +0000
+++ b/main.cpp	Fri Feb 09 18:37:11 2018 +0000
@@ -28,7 +28,7 @@
 
     while(1)
     {
-        consoleUI2(); 
+        consoleUI(); 
         Thread::wait(500); // Go to sleep for 500 ms  
     }
 }
--- a/ui.cpp	Sat Feb 03 00:48:47 2018 +0000
+++ b/ui.cpp	Fri Feb 09 18:37:11 2018 +0000
@@ -60,6 +60,7 @@
 /******************************************************************************
                            User interface
 ******************************************************************************/
+/*
 void consoleUI(void)
 {
     if (bluetooth.readable()) {
@@ -113,12 +114,12 @@
     bluetooth.printf("\r\nPos: %d, dP: %d, dT: %d, Kp: %f, Ki: %f, vel: %d, e: %d", position, dPosition, xState, dTime, Kp, Ki, vel, e);
     
 }
-
+*/
 /******************************************************************************
                            User interface 2
 ******************************************************************************/
 
-void consoleUI2(void)
+void consoleUI(void)
 {
      
     if (bluetooth.readable()) {
@@ -139,7 +140,7 @@
             if ( setpoint < 506 ) 
             {
                 //setpoint = setpoint + SPEED_STEP;
-                setpoint = 200;
+                setpoint = 400;
             }
             setpoint_mutex.unlock();
 
@@ -154,7 +155,7 @@
             setpoint_mutex.lock();
             if (setpoint > -560) 
             {
-                setpoint = -200;
+                setpoint = -400;
                 //setpoint = setpoint - SPEED_STEP;
             }
                 
@@ -163,21 +164,21 @@
             // display speed
             bluetooth.printf("\r\n %5d", setpoint);
         }
-        else if (x=='e')
+        else if (x=='i')
         {
-            Ki = Ki + 0.005;
+            Ki = Ki + 0.001;
         }
-        else if (x=='d')
+        else if (x=='k')
         {
-            Ki = Ki - 0.005;
+            Ki = Ki - 0.001;
         }
-        else if (x=='t')
+        else if (x=='o')
         {
-            Kp = Kp + 0.05;
+            Kp = Kp + 0.01;
         }
-        else if (x=='g')
+        else if (x=='l')
         {
-            Kp = Kp - 0.05;    
+            Kp = Kp - 0.01;    
         }
         
         // error wrong input
--- a/ui.h	Sat Feb 03 00:48:47 2018 +0000
+++ b/ui.h	Fri Feb 09 18:37:11 2018 +0000
@@ -3,17 +3,8 @@
 
 #define SPEED_STEP     1
 
-
-
-
 void consoleUI(void);
-void consoleUI2(void);
 void displayStartupMsg(void);
 void twoTerminalsTest(void);
 
-
-
-
-
-
 #endif
\ No newline at end of file