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:
Sat Feb 03 00:05:08 2018 +0000
Parent:
0:a355e511bc5d
Child:
2:ca2a7430739b
Commit message:
setpoint represent velocity

Changed in this revision

Drivers/DE0_driver.cpp Show annotated file Show diff for this revision Revisions of this file
Drivers/DE0_driver.h Show annotated file Show diff for this revision Revisions of this file
Drivers/motor_driver.cpp Show annotated file Show diff for this revision Revisions of this file
Drivers/motor_driver.h Show annotated file Show diff for this revision Revisions of this file
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
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/Drivers/DE0_driver.cpp	Thu Feb 01 03:58:00 2018 +0000
+++ b/Drivers/DE0_driver.cpp	Sat Feb 03 00:05:08 2018 +0000
@@ -13,6 +13,19 @@
 DigitalOut SpiReset(p14);   // 0-1-0 tells the DE0 SPI interace that the next word sent is a control word
 
 
+int SignExtend(int16_t x)
+{
+
+    // if number is negative
+    if(x&0x00008000) {
+        // reserve the sign bit into the 32-bit number
+        x = x|0xFFFF0000;
+    }
+
+    return x;
+}
+
+
 // To reset all SPI peripheral on the FPGA:
 void ResetDE0(void){
     IoReset=0; // Reset all DE0 peripherals
@@ -67,7 +80,7 @@
  */
 
 
-void DE0_read(uint16_t * id, uint16_t * dP, uint16_t * dT)
+void DE0_read(uint16_t * id, int * dP, uint16_t * dT)
 {
     // To place SPI module into control mode, where the next word received by the
     // slave is interpreted as a control word.
@@ -77,8 +90,8 @@
     *id = (uint16_t)DE0.write(0x8002);
     
     // Read the two 16 bits registers from the FPGA 
-    *dP = (uint16_t)DE0.write(DUMMY); // A SPI read only transaction occurs.
-    *dT = (uint16_t)DE0.write(DUMMY); //
+    *dP = SignExtend(DE0.write(DUMMY)); // A SPI read only transaction occurs.
+    *dT = DE0.write(DUMMY); //
 }
 
  
--- a/Drivers/DE0_driver.h	Thu Feb 01 03:58:00 2018 +0000
+++ b/Drivers/DE0_driver.h	Sat Feb 03 00:05:08 2018 +0000
@@ -5,7 +5,7 @@
 
 
 void DE0_init(void);
-void DE0_read(uint16_t *, uint16_t * , uint16_t *);
+void DE0_read(uint16_t *, int * , uint16_t *);
 
 
 
--- a/Drivers/motor_driver.cpp	Thu Feb 01 03:58:00 2018 +0000
+++ b/Drivers/motor_driver.cpp	Sat Feb 03 00:05:08 2018 +0000
@@ -22,7 +22,7 @@
     // motor is still uninitialized
     motor_status = MOTOR_UNINIT;
 
-    PwmR.period_ms(PERIOD); // This sets the PWM period in us
+    PwmR.period_us(PERIOD); // This sets the PWM period in us
     // DC Motor Control Pins
     DirR = 0;
     PwmR = 0;
--- a/Drivers/motor_driver.h	Thu Feb 01 03:58:00 2018 +0000
+++ b/Drivers/motor_driver.h	Sat Feb 03 00:05:08 2018 +0000
@@ -3,7 +3,7 @@
 #define MOTOR_DRIVER_H
 
 //motor configuration
-#define PERIOD  20000     // period in ms -> frequency = 50kHz -- audible frequency is between 20Hz - 20kHz
+#define PERIOD  40     // period in us -> frequency = 25kHz -- audible frequency is between 20Hz - 20kHz
 
 typedef enum { MOTOR_UNINIT,
                MOTOR_INIT,
--- a/PiControlThread.cpp	Thu Feb 01 03:58:00 2018 +0000
+++ b/PiControlThread.cpp	Sat Feb 03 00:05:08 2018 +0000
@@ -2,19 +2,30 @@
 #include "ui.h"
 #include "Drivers/motor_driver.h"
 #include "Drivers/DE0_driver.h"
+#include "PiControlThread.h"
 
 // global speed variable;
 extern int setpoint;
 extern Serial pc;
 extern Mutex setpoint_mutex;
 
-uint16_t ID, dPosition, dTime;
-int e, u, xState;
+uint16_t ID, dTime;
+int dPosition;
+int vel;
 
-float Ki, Kp;
+int32_t e, u, xState;
+
+bool saturationFlag;
+
+float Ki; 
+float Kp;
 
 void PiControlThread(void const *);
 void PeriodicInterruptISR(void);
+int SaturatingSubtract(int , int );
+int SaturatingAdd(int , int );
+int SaturateValue(int , int );
+
 
 osThreadId PiControlId;
 
@@ -45,7 +56,11 @@
 
     // Specify address of the PeriodicInt ISR as PiControllerISR, specify the interval
     // in seconds between interrupts, and start interrupt generation:
-    PeriodicInt.attach(&PeriodicInterruptISR, 0.05); 
+    PeriodicInt.attach(&PeriodicInterruptISR, 0.05);   // 50ms sampling rate
+    
+    Kp = 2;
+    Ki = 0.01;
+    
 }
 
 
@@ -53,23 +68,48 @@
 *               ******** Periodic Timer Interrupt Thread ********
 *******************************************************************************/
 void PiControlThread(void const *argument)
-{  
+{
+    // initialization
+    saturationFlag = false;
+    int scale = 40;
+      
     while (true) 
     {
-        
         osSignalWait(0x01, osWaitForever); // Go to sleep until signal, SignalPi, is received.
-        led3= !led3; // Alive status - led3 toggles each time PieriodicZInterruptsThread is signaled.
         
         // get incremental position and time from QEI
         DE0_read(&ID, &dPosition, &dTime);
+        SaturateValue(dPosition, 560);
+        
+        // maximum velocity at dPostition = 560 is vel = 703
+        vel = (float)((6135.92 * dPosition) / dTime) ;
         
         setpoint_mutex.lock();
-        e = setpoint-dPosition;  // e is the velocity error
+        e = SaturatingSubtract(setpoint, vel);  // e is the velocity error
         setpoint_mutex.unlock();
         
-        xState = xState + e; // x is the Euler approximation to the integral of e.
-        u = Kp*e + Ki*xState; // u is the control signal
-              
+        // disable integration if u is saturated
+        if(saturationFlag)
+        {
+            xState = xState;
+        }
+        else
+        {
+            xState = SaturatingAdd(xState, e); // x is the Euler approximation to the integral of 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
+        
+        u = (float)(Kp*e/scale) + (float)(Ki*xState/scale); // u is the control signal      
+        
+        // if u is saturated set the flag to true
+        if( (u>= U_LIMIT) || (u<= -U_LIMIT) )  saturationFlag = true;
+        else saturationFlag = false;
+        
+        // saturate u at values greater than 20 and smaller than -20
+        u = SaturateValue(u, U_LIMIT);
         
         if (u >= 0)
         {
@@ -78,12 +118,10 @@
         else if (u < 0)
         {
             motorDriver_reverse(u);
-        }
-        else
-        {
-            pc.printf("\r\nerror!!!");
-        }
+        }   
+        
     }
+    
 }
 
 /*******************************************************************************
@@ -96,3 +134,32 @@
     // Send signal to the thread with ID, PeriodicInterruptId, i.e., PeriodicInterruptThread.
     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	Thu Feb 01 03:58:00 2018 +0000
+++ b/PiControlThread.h	Sat Feb 03 00:05:08 2018 +0000
@@ -2,6 +2,7 @@
 #ifndef PERIODIC_INT_H
 #define PERIODIC_INT_H
 
+#define U_LIMIT     20
 
 void PiControlThreadInit(void);
 
--- a/main.cpp	Thu Feb 01 03:58:00 2018 +0000
+++ b/main.cpp	Sat Feb 03 00:05:08 2018 +0000
@@ -28,7 +28,7 @@
 
     while(1)
     {
-        consoleUI(); 
+        consoleUI2(); 
         Thread::wait(500); // Go to sleep for 500 ms  
     }
 }
--- a/ui.cpp	Thu Feb 01 03:58:00 2018 +0000
+++ b/ui.cpp	Sat Feb 03 00:05:08 2018 +0000
@@ -8,14 +8,21 @@
 Mutex setpoint_mutex;
 
 // speed
-int setpoint;
+int setpoint = 0;
 
 // variable to store character recieved from terminal
 char x;
 
-extern uint16_t dPosition, dTime;
+extern int16_t dPosition, dTime;
+
+extern float Ki; 
+extern float Kp;
 
-uint16_t position;
+extern int vel;
+
+int16_t position;
+
+extern int32_t e;
 
 /******************************************************************************
             A function to test blutooth communication
@@ -40,63 +47,146 @@
 ******************************************************************************/
 void displayStartupMsg()
 {
-    pc.printf("\r\n************************************");
-    pc.printf("\r\n**** DC Motor Control using PWM ****");
-    pc.printf("\r\n************************************");
-    pc.printf("\r\n-Enter r to reset the watchdog timer");
-    pc.printf("\r\n-press w to increase motor speed");
-    pc.printf("\r\n-press s to decrease motor speed");
+    bluetooth.printf("\r\n************************************");
+    bluetooth.printf("\r\n**** DC Motor Control using PWM ****");
+    bluetooth.printf("\r\n************************************");
+    bluetooth.printf("\r\n-Enter r to reset the watchdog timer");
+    bluetooth.printf("\r\n-press w to increase motor speed");
+    bluetooth.printf("\r\n-press s to decrease motor speed");
 }
 
 
 /******************************************************************************
-                            Console interface
+                           User interface
 ******************************************************************************/
 void consoleUI(void)
 {
-    if (pc.readable()) {
-        x = pc.getc();
-
+    if (bluetooth.readable()) {
+        x = bluetooth.getc();
+        
         // if input from console is the letter 'r'
         if(x == 'r') {
             // reset watchdog timer
             WatchdogReset();
-            pc.printf("\r\nWatchdog has been reset");
+            bluetooth.printf("\r\nWatchdog has been reset");
         }
 
         // if w is pressed increase the speed
         // by incrementing u
         else if(x == 'w') {
-            // convert to integer
-            if(setpoint<=10000) {
-                setpoint_mutex.lock();
-                setpoint = setpoint + SPEED_STEP;
-                setpoint_mutex.unlock();
+            setpoint_mutex.lock();
+            if ( setpoint < 560 ) 
+            {
+                //setpoint = setpoint + SPEED_STEP;
+                setpoint = 100;
             }
+            setpoint_mutex.unlock();
+
             // display speed
-            pc.printf("\r\n %5d", setpoint);
+            bluetooth.printf("\r\n %5d", setpoint);
         }
 
         // if s is pressed decrease the speed
         // by decrementing u
         else if(x == 's') {
-            // convert to integer
-            if(setpoint>-10000) {
-                setpoint_mutex.lock();
-                setpoint = setpoint - SPEED_STEP;
-                setpoint_mutex.unlock();
+
+            setpoint_mutex.lock();
+            if (setpoint > -560) 
+            {
+                setpoint = -100;
+                //setpoint = setpoint - SPEED_STEP;
             }
+                
+            setpoint_mutex.unlock();
+
             // display speed
-            pc.printf("\r\n %5d", setpoint);
+            bluetooth.printf("\r\n %5d", setpoint);
         }
 
         // error wrong input
         else {
-            pc.printf("\r\nwrong input please enter \'w\' to increase the speed, \'s\' to reduce it or move in the opposite direction and \'r\' to reset the watchdog");
+            bluetooth.printf("\r\nwrong input please enter \'w\' to increase the speed, \'s\' to reduce it or move in the opposite direction and \'r\' to reset the watchdog");
         }
     }
     position += dPosition;
-    pc.printf("\r\nposition: %d, dPosition: %d, dTime: %d", position, dPosition, dTime);
+    bluetooth.printf("\r\nPos: %d, dP: %d, dT: %d, Kp: %f, Ki: %f, vel: %d, e: %d", position, dPosition, dTime, Kp, Ki, vel, e);
     
 }
 
+/******************************************************************************
+                           User interface 2
+******************************************************************************/
+
+void consoleUI2(void)
+{
+     
+    if (bluetooth.readable()) {
+        x = bluetooth.getc();
+        
+        // if input from console is the letter 'r'
+        if(x == 'r') {
+            // reset watchdog timer
+            WatchdogReset();
+            setpoint = 0;
+            bluetooth.printf("\r\nWatchdog has been reset");
+        }
+
+        // if w is pressed increase the speed
+        // by incrementing u
+        else if(x == 'w') {
+            setpoint_mutex.lock();
+            if ( setpoint < 703 ) 
+            {
+                //setpoint = setpoint + SPEED_STEP;
+                setpoint = 200;
+            }
+            setpoint_mutex.unlock();
+
+            // display speed
+            bluetooth.printf("\r\n %5d", setpoint);
+        }
+
+        // if s is pressed decrease the speed
+        // by decrementing u
+        else if(x == 's') {
+
+            setpoint_mutex.lock();
+            if (setpoint > -703) 
+            {
+                setpoint = -200;
+                //setpoint = setpoint - SPEED_STEP;
+            }
+                
+            setpoint_mutex.unlock();
+
+            // display speed
+            bluetooth.printf("\r\n %5d", setpoint);
+        }
+        else if (x=='e')
+        {
+            Ki = Ki + 0.005;
+        }
+        else if (x=='d')
+        {
+            Ki = Ki - 0.005;
+        }
+        else if (x=='t')
+        {
+            Kp = Kp + 0.05;
+        }
+        else if (x=='g')
+        {
+            Kp = Kp - 0.05;    
+        }
+        
+        // error wrong input
+        else {
+            bluetooth.printf("\r\nwrong input please enter \'w\' to increase the speed, \'s\' to reduce it or move in the opposite direction and \'r\' to reset the watchdog");
+        }
+    }   
+    
+    position += dPosition;
+    //bluetooth.printf("\r\nPos: %d, dP: %d, dT: %d, Kp: %f, Ki: %f, vel: %d, e: %d", position, dPosition, dTime, Kp, Ki, vel, e);
+    bluetooth.printf("\r\nKp: %f, Ki: %f, vel: %d, e: %d", Kp, Ki, vel, e);
+    
+}
--- a/ui.h	Thu Feb 01 03:58:00 2018 +0000
+++ b/ui.h	Sat Feb 03 00:05:08 2018 +0000
@@ -1,12 +1,13 @@
 #ifndef UI_H
 #define UI_H
 
-#define SPEED_STEP     100
+#define SPEED_STEP     1
 
 
 
 
 void consoleUI(void);
+void consoleUI2(void);
 void displayStartupMsg(void);
 void twoTerminalsTest(void);