Diff: PiControlThread.cpp

Revision:

5:b29220d29022

Parent:

4:417e475239c7

--- a/PiControlThread.cpp	Fri Feb 09 18:37:11 2018 +0000
+++ b/PiControlThread.cpp	Fri Feb 09 20:26:04 2018 +0000
@@ -6,11 +6,16 @@
 #include "PiController.h"
 
 extern int setpoint;
+extern bool start_counting;
+extern Serial bluetooth;
 
 uint16_t ID, dTime;
 int dPosition;
 int vel;
 int32_t U;
+int counter =0 ;
+
+int dPosition_values[1000];
 
 void PiControlThread(void const *);
 void PeriodicInterruptISR(void);
@@ -28,7 +33,7 @@
 /******************************************************************************/
 
 // Declare PeriodicInterruptThread as a thread/process
-osThreadDef(PiControlThread, osPriorityRealtime, 1024); 
+osThreadDef(PiControlThread, osPriorityRealtime, 1024);
 
 Ticker PeriodicInt;      // Declare a timer interrupt: PeriodicInt
 
@@ -40,13 +45,13 @@
     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
-     
+    PeriodicInt.attach(&PeriodicInterruptISR, 0.001);   // 50ms sampling rate
+
 }
 
 
@@ -55,41 +60,69 @@
 *******************************************************************************/
 void PiControlThread(void const *argument)
 {
-    
-    while (true) 
-    {
+
+    // time constant calc
+    setpoint = 140;
+
+    while (true) {
         osSignalWait(0x01, osWaitForever); // Go to sleep until signal, SignalPi, is received.
-        
+
         // 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) ;
-        
-        U = PiController(setpoint,dPosition);
-        
-        if (U >= 0)
-        {
+
+        //U = PiController(setpoint,dPosition);
+
+        // time constnant calc.
+        U = setpoint/28;
+
+        if (U >= 0) {
             motorDriver_forward(U);
+        } else if (U < 0) {
+            motorDriver_reverse(U);
         }
-        else if (U < 0)
-        {
-            motorDriver_reverse(U);
-        }   
-           
+
+
+        // start our calculations
+        if( start_counting == true ) {
+            float time_constant = 0;
+            dPosition_values[counter] = dPosition;
+            counter++;
+            
+            // if we reach our setpoint
+            if (dPosition > 8 ) {
+                // stop counting time
+                start_counting = false;
+                // find the value where dPosition = 316
+                for(int i=0; i<1000; i++) {
+                    if( dPosition_values[i] >= 7 ) {
+                        time_constant = counter;
+                        //time_constant = time_constant;
+                        bluetooth.printf("\ntime constant: %f", time_constant);
+                    }
+                }
+            }
+        }
+
+
+
+
+
     }
-    
+
 }
 
 /*******************************************************************************
-* the interrupt below occures every 250ms as setup in the main function during 
+* 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); 
+    osSignalSet(PiControlId,0x1);
 }