Diff: PiControlThread.cpp

Revision:

0:a355e511bc5d

Child:

1:3e9684e81312

diff -r 000000000000 -r a355e511bc5d PiControlThread.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PiControlThread.cpp	Thu Feb 01 03:58:00 2018 +0000
@@ -0,0 +1,98 @@
+#include "mbed.h"
+#include "ui.h"
+#include "Drivers/motor_driver.h"
+#include "Drivers/DE0_driver.h"
+
+// global speed variable;
+extern int setpoint;
+extern Serial pc;
+extern Mutex setpoint_mutex;
+
+uint16_t ID, dPosition, dTime;
+int e, u, xState;
+
+float Ki, Kp;
+
+void PiControlThread(void const *);
+void PeriodicInterruptISR(void);
+
+osThreadId PiControlId;
+
+/******************************************************************************/
+//  osPriorityIdle          = -3,          ///< priority: idle (lowest)
+//  osPriorityLow           = -2,          ///< priority: low
+//  osPriorityBelowNormal   = -1,          ///< priority: below normal
+//  osPriorityNormal        =  0,          ///< priority: normal (default)
+//  osPriorityAboveNormal   = +1,          ///< priority: above normal
+//  osPriorityHigh          = +2,          ///< priority: high
+//  osPriorityRealtime      = +3,          ///< priority: realtime (highest)
+/******************************************************************************/
+
+// Declare PeriodicInterruptThread as a thread/process
+osThreadDef(PiControlThread, osPriorityRealtime, 1024); 
+
+Ticker PeriodicInt;      // Declare a timer interrupt: PeriodicInt
+
+DigitalOut led3(LED3);
+
+
+void PiControlThreadInit()
+{
+    DE0_init();             // initialize FPGA
+    motorDriver_init();     // initialize motorDriver
+    
+    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); 
+}
+
+
+/*******************************************************************************
+*               ******** Periodic Timer Interrupt Thread ********
+*******************************************************************************/
+void PiControlThread(void const *argument)
+{  
+    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);
+        
+        setpoint_mutex.lock();
+        e = setpoint-dPosition;  // 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
+              
+        
+        if (u >= 0)
+        {
+            motorDriver_forward(u);
+        }
+        else if (u < 0)
+        {
+            motorDriver_reverse(u);
+        }
+        else
+        {
+            pc.printf("\r\nerror!!!");
+        }
+    }
+}
+
+/*******************************************************************************
+* the interrupt below occures every 250ms as setup in the main function during 
+* initialization
+*               ******** Period Timer Interrupt Handler ********
+*******************************************************************************/
+void PeriodicInterruptISR(void)
+{
+    // Send signal to the thread with ID, PeriodicInterruptId, i.e., PeriodicInterruptThread.
+    osSignalSet(PiControlId,0x1); 
+}