Diff: main.cpp

Revision:

5:7108ac9e8182

Parent:

4:03bf5bdca9fb

Child:

6:5200ce9fa5a7

--- a/main.cpp	Fri Feb 22 20:57:43 2013 +0000
+++ b/main.cpp	Mon Feb 25 21:07:42 2013 +0000
@@ -8,10 +8,10 @@
 // --- Constants
 #define Dummy 0
 #define PWMPeriod 0.0005 // orignally 0.001
+// Control Update = 50ms (time should be 609/610) (if we want to change this we also have to change the way feedback speed is calculated)
 #define ControlUpdate 0.05
 #define EncoderTime 610
-#define Kp = 1.2;
-#define Ki = 1.2;
+
 
 // --- Function prototypes
 void PiControllerISR(void);
@@ -30,12 +30,19 @@
 Mutex Var_Lock;
 
 // Global variables for interrupt handler
-float u1;
-float u2;
+float u1 = 0; 
+float u2 = 0;
+float userSetL = 0;
+float userSetR = 0;
+float prevu1, prevu2;
 int dPositionRight, dTimeRight, dPositionLeft, dTimeLeft;
 int startup = 0;
 float aeL = 0; 
 float aeR = 0;
+float eL = 0;
+float eR = 0;
+float fbSpeedL;
+float fbSpeedR;
 
 // --- Processes and threads
 int32_t SignalPi, SignalWdt, SignalExtCollision;
@@ -88,7 +95,7 @@
 {    
     InitializeSystem();
     
-    pc.printf("\r\n Robot Initialization Complete \r\n");    
+    pc.printf("\r\n --- Robot Initialization Complete --- \r\n");    
     
     BluetoothSerial.printf("\n\n\rTap w-a-s-d keys for differential speed control: ");
     
@@ -100,7 +107,12 @@
         pc.printf("Left Position: %d \n\r", dPositionLeft);
         pc.printf("Left Time: %d \n\r", dTimeLeft);
         pc.printf("Right Position: %d \n\r", dPositionRight);
-        pc.printf("Right Time: %d \n\n\r", dTimeRight);
+        pc.printf("Right Time: %d \n\r", dTimeRight);
+        pc.printf("Feedback Speed Left: %f \n\r", fbSpeedL);
+        pc.printf("Feedback Speed Right: %f \n\r", fbSpeedR);
+        pc.printf("Left u: %f Right u: %f\r\n", u1, u2);
+        pc.printf("Left e: %f Right e: %f\r\n", eL, eR);
+        pc.printf("Left Ae: %f Right Ae: %f\n\r\n", aeL, aeR);
         Var_Lock.unlock();
         
         /*if (pc.readable()){
@@ -112,11 +124,9 @@
         if(pc.readable()) 
         {
             pc.printf("\n\r Enter a motor speed (with sign as direction:\n\r");
-            pc.scanf("%f", &u1);
-            pc.printf("%f", u1);
-            u2 = u1;
-            
-            
+            pc.scanf("%f", &userSetL);
+            pc.printf("%f", userSetL);
+            userSetR = userSetL;
             
             /* x=pc.getc();
             if(x=='w')
@@ -159,25 +169,86 @@
         // Read encoder and display results
         ReadEncoder();
         
-        float fbSpeedL;
-        float fbSpeedR;
-        float eL = 0;
-        float eR = 0;
+        //float fbSpeedL;
+        //float fbSpeedR;
+        //float eL = 0;
+        //float eR = 0;
+        float maxError = 1000;
+        float maxAcc = 10000;
+        float Kp = 1.2;
+        float Ki = 1.2;
+        float leftPos = (float)dPositionLeft;
+        float rightPos = (float)dPositionRight;
+        float leftMaxPos = 1438.0f;
+        float rightMaxPos = 1484.0f;
+        
+        prevu1 = u1;
+        prevu2 = u2;
         
         // calculate feedback speed percentage
-        fbSpeedL = dPositionLeft/1438; 
-        fbSpeedR = dPositionRight/1484;
+        // ****** TODO : BOUND FEEDABCK SPEED
+        fbSpeedL = (leftPos/leftMaxPos); 
+        fbSpeedR = (rightPos/rightMaxPos);
         
         // calculate error
-        eL = u1 - fbSpeedL;
-        eR = u2 - fbSpeedR;
+        eL = userSetL - fbSpeedL;
+        eR = userSetR - fbSpeedR;
+        //eL = -eL;
+        //eR = -eR;
+        // prevent overflow / bound the error
+        /*
+        if (eL > maxError)
+        {
+            eL = maxError;
+        }
+        if (eL < -maxError);
+        {
+            eL = -maxError;
+        }
+        if (eR > maxError)
+        {
+            eR = maxError;
+        }
+        if (eR < -maxError);
+        {
+            eR = -maxError;
+        }
+        */
         
         // accumulated error (integration)
-        aeL += eL;
-        aeR += eR;
+        if (prevu1 < 1 && prevu1 > -1)
+        {    
+            aeL += eL;
+        }
+        if (prevu2 < 1 && prevu2 > -1)
+        {
+            aeR += eR;
+        }
+        
+        // bound the accumulatd error
+        /*
+        if (aeL > maxAcc)
+        {
+            aeL = maxAcc;
+        }
+        if (aeL < -maxAcc)
+        {
+            aeL = -maxAcc;
+        }
+        if (aeR > maxAcc)
+        {
+            aeR = maxAcc;
+        }
+        if (aeR < -maxAcc)
+        {
+            aeR = -maxAcc;
+        }
+        */
         
         u1 = Kp*eL + Ki*aeL;
         u2 = Kp*eR + Ki*aeR;
+        
+        
         // Is signaled by a periodic timer interrupt handler
         /*
         Read incremental position, dPosition, and time interval from the QEI.
@@ -188,6 +259,14 @@
         Update the DIR pin on the LMD18200 with the sign of u.
         */
         
+        /*
+        pc.printf("Feedback Speed Left: %f \n\r", fbSpeedL);
+        pc.printf("Feedback Speed Right: %f \n\r\n", fbSpeedR);
+        
+        u1 = userSetL;
+        u2 = u1;
+        */
+        
         SetLeftMotorSpeed(u1);
         SetRightMotorSpeed(u2);
     } 
@@ -211,7 +290,7 @@
 }
 
 // ******** Period Timer Interrupt Handler ********
-void PiControllerISR(void) 
+void PiControllerISR(void)
 {
     osSignalSet(PiControl,0x1);
 }