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 20:26:04 2018 +0000
Parent:
4:417e475239c7
Commit message:
time constant calculation

Changed in this revision

PiControlThread.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
--- 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);
 }
 
--- a/ui.cpp	Fri Feb 09 18:37:11 2018 +0000
+++ b/ui.cpp	Fri Feb 09 20:26:04 2018 +0000
@@ -23,6 +23,7 @@
 
 int16_t position;
 
+bool start_counting = false;
 
 
 /******************************************************************************
@@ -140,7 +141,8 @@
             if ( setpoint < 506 ) 
             {
                 //setpoint = setpoint + SPEED_STEP;
-                setpoint = 400;
+                setpoint = 420;
+                start_counting = true;
             }
             setpoint_mutex.unlock();
 
@@ -191,8 +193,12 @@
     
     //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\ndP: %d, vel: %d, Kp: %f, Ki: %f, e: %d, xState: %d", dPosition, vel, Kp, Ki, e, xState);
+    //bluetooth.printf("\r\ndP: %d, vel: %d, Kp: %f, Ki: %f, e: %d, xState: %d", dPosition, vel, Kp, Ki, e, xState);
     
     //bluetooth.printf("\r\ne: %d, Pos: %d, dP: %d, xState: %d, u: %d, dT: %d", e, position, dPosition, xState, u, dTime);
     
+     bluetooth.printf("\r\n%d", dPosition);
+     
+     
+    
 }