ManualControl

Dependencies:   TPixy-Interface

Fork of MbedOS_Robot by ECE4333 - 2018 - Ahmed & Brandon

Files at this revision

API Documentation at this revision

Comitter:
asobhy
Date:
Thu Feb 15 01:15:26 2018 +0000
Parent:
6:e7ce340fe91e
Child:
8:a0890fa79084
Commit message:
Added comments to several files and organized the code a bit

Changed in this revision

Drivers/DE0_driver.cpp Show annotated file Show diff for this revision Revisions of this file
Drivers/PiController.cpp Show annotated file Show diff for this revision Revisions of this file
Drivers/PiController.h Show annotated file Show diff for this revision Revisions of this file
ExternalInterruptThread.cpp 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
PiController.cpp Show diff for this revision Revisions of this file
PiController.h Show diff for this revision Revisions of this file
mbed-os.lib Show annotated file Show diff for this revision Revisions of this file
--- a/Drivers/DE0_driver.cpp	Sat Feb 10 19:35:21 2018 +0000
+++ b/Drivers/DE0_driver.cpp	Thu Feb 15 01:15:26 2018 +0000
@@ -15,13 +15,11 @@
 
 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;
 }
 
@@ -43,6 +41,14 @@
     SpiReset=0;
     }
 
+
+/*******************************************************************************
+* @brief    This is the initialization function for the DE0 FPGA - communication
+            between the FPGA and MCU is done through SPI. The function 
+            initializes the SPI channel to a bit rate of 500kbps
+* @param    none
+* @return   none
+*******************************************************************************/
 void DE0_init(void)
 {
  DE0.format(16,1); // Define SPI format: 16-bit words, mode 1 protocol.
@@ -50,8 +56,7 @@
  ResetDE0();
 }
 
-/*
-
+/*******************************************************************************
 Before an SPI data transaction with the DE0 FPGA can occur, a control word must 
 first be written to the slave that specifies: 
 
@@ -62,24 +67,27 @@
 
 A control word need only be written once – if the same transactions are repeated.
 Subsequent transactions use the protocol specified by the last control word that
- was written. To change the transaction protocol a new control needs to be
- written. To write a new control word, the SpiReset input must first be set then
- cleared followed by a SPI write of a 16-bit word that is interpreted as the
- control word by the slave.
+was written. To change the transaction protocol a new control needs to be
+written. To write a new control word, the SpiReset input must first be set then
+cleared followed by a SPI write of a 16-bit word that is interpreted as the
+control word by the slave.
  
- The 16-bit control word format appears below.
+The 16-bit control word format appears below.
 
 1        7bits            8bits
 rd | address offset | number of words 
 
-
 When RD = 1, no write operation occurs within the slave. Data on the MOSI is 
 ignored by the slave. When RD = 0, a simultaneous read + write of the slave 
 occurs.
- 
- */
+*******************************************************************************/
 
 
+/*******************************************************************************
+* @brief    This function reads the output from the FPGA through the SPI channel
+* @param    none
+* @return   none
+*******************************************************************************/
 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
@@ -93,7 +101,4 @@
     *dP = SignExtend(DE0.write(DUMMY)); // A SPI read only transaction occurs.
     *dT = DE0.write(DUMMY); //
 }
-
- 
-
     
\ No newline at end of file
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Drivers/PiController.cpp	Thu Feb 15 01:15:26 2018 +0000
@@ -0,0 +1,88 @@
+#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;  
+        
+}
+
+
+/*******************************************************************************
+* @brief    PI Controller function
+* @param    setp is the setpoint we would like the system to get to
+* @param    dP is the current speed of the system 
+* @return   u is the control signal out of the controller to make the system 
+*           reach the desired setpoint
+*******************************************************************************/
+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
+    // scaling factor used is 40
+    
+    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;
+    
+}
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Drivers/PiController.h	Thu Feb 15 01:15:26 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/ExternalInterruptThread.cpp	Sat Feb 10 19:35:21 2018 +0000
+++ b/ExternalInterruptThread.cpp	Thu Feb 15 01:15:26 2018 +0000
@@ -12,14 +12,23 @@
 DigitalOut led2(LED2);
 
 
+/*******************************************************************************
+* @brief    This is the initialization function for ExternalInterruptthread
+* @param    none
+* @return   none
+*******************************************************************************/
 void ExternalInterruptThreadInit()
 {
     Bumper.rise(&ExtInterruptISR); // Attach the address of the interrupt handler to the rising edge of Bumper
     ExtInterruptId = osThreadCreate(osThread(ExtInterruptThread), NULL);            
 }
 
-
-// ******** External Interrupt Thread ********
+ 
+/*******************************************************************************
+* @brief    ******** External Interrupt Thread ********
+* @param    none
+* @return   none
+*******************************************************************************/
 void ExtInterruptThread(void const *argument)
 {
     while (true) {
@@ -29,7 +38,11 @@
 }
 
 
-// ******** External Interrupt Handler ********
+/*******************************************************************************
+* @brief    ******** External Interrupt ISR ********
+* @param    none
+* @return   none
+*******************************************************************************/
 void ExtInterruptISR(void)
 {
     osSignalSet(ExtInterruptId,0x01); // Send signal to the thread with ID, ExtInterruptId, i.e., ExtInterruptThread.
--- a/PiControlThread.cpp	Sat Feb 10 19:35:21 2018 +0000
+++ b/PiControlThread.cpp	Thu Feb 15 01:15:26 2018 +0000
@@ -3,7 +3,7 @@
 #include "Drivers/motor_driver.h"
 #include "Drivers/DE0_driver.h"
 #include "PiControlThread.h"
-#include "PiController.h"
+#include "Drivers/PiController.h"
 
 extern int setpoint;
 
@@ -34,7 +34,13 @@
 
 DigitalOut led3(LED3);
 
-
+/*******************************************************************************
+* @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
+*******************************************************************************/
 void PiControlThreadInit()
 {
     DE0_init();                  // initialize FPGA
@@ -52,7 +58,10 @@
 
 
 /*******************************************************************************
-*               ******** Periodic Timer Interrupt Thread ********
+* @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)
 {
@@ -77,16 +86,17 @@
         else if (U < 0)
         {
             motorDriver_reverse(U);
-        }   
+        }
            
     }
     
 }
 
 /*******************************************************************************
-* the interrupt below occures every 250ms as setup in the main function during 
-* initialization
-*               ******** Period Timer Interrupt Handler ********
+* @brief    The ISR below signals the PIControllerThread. it is setup to run 
+*           every 50ms
+* @param    none
+* @return   none
 *******************************************************************************/
 void PeriodicInterruptISR(void)
 {
--- a/PiController.cpp	Sat Feb 10 19:35:21 2018 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,86 +0,0 @@
-#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
--- a/PiController.h	Sat Feb 10 19:35:21 2018 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,13 +0,0 @@
-#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/mbed-os.lib	Sat Feb 10 19:35:21 2018 +0000
+++ b/mbed-os.lib	Thu Feb 15 01:15:26 2018 +0000
@@ -1,1 +1,1 @@
-https://github.com/ARMmbed/mbed-os/#96d9a00d0a1d25095b330095fa81c40f7741777c
+https://github.com/ARMmbed/mbed-os/#569159b784f70feaa32ce226aaca896fb83452f7