Diff: main.cpp

Revision:

7:751d5e3e9cab

Parent:

6:5200ce9fa5a7

Child:

8:d65cba3bfc0e

--- a/main.cpp	Tue Feb 26 19:58:06 2013 +0000
+++ b/main.cpp	Wed Feb 27 21:46:23 2013 +0000
@@ -8,8 +8,8 @@
 // --- Constants
 #define Dummy 0
 
-//#define PWMPeriod 0.0005 // orignally 0.001
-const float PWMPeriod = 0.0005;
+//#define PWMPeriod 0.0005 // orignally 0.001 (gave a high pitched whine)
+const float PWMPeriod = 0.00005;
 // 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
 const float ControlUpdate = 0.05;
@@ -27,13 +27,15 @@
 void InitializeEncoder();
 void InitializePWM();
 void PwmSetOut(float d, float T);
-void GetSpeeds(float *leftSpeed, float *rightSpeed);
+void GetSpeeds();
 void SetLeftMotorSpeed(float u);
 void SetRightMotorSpeed(float u);
 
 // Global variables
 float u1 = 0; 
 float u2 = 0;
+float cSetL = 0;
+float cSetR = 0;
 float userSetL = 0;
 float userSetR = 0;
 int startup = 0;
@@ -42,7 +44,7 @@
 Mutex Var_Lock;
 
 // global variables to eventually be removed
-unsigned int dPositionLeft, dTimeLeft, dPositionRight, dTimeRight;
+short dPositionLeft, dTimeLeft, dPositionRight, dTimeRight;
 float fbSpeedL, fbSpeedR;
 float eL = 0;
 float eR = 0;
@@ -104,17 +106,14 @@
     
     while(1)
     {
-        Var_Lock.lock();
-        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\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();
+        //Var_Lock.lock();
+        pc.printf("Pos. L: %d R: %d \n\r", dPositionLeft, dPositionRight);
+        pc.printf("Time L: %d R: %d \n\r", dTimeLeft, dTimeRight);
+        pc.printf("fbs  L: %f R: %f \n\r", fbSpeedL, fbSpeedR);
+        pc.printf("e    L: %f R: %f \r\n", eL, eR);
+        pc.printf("Ae   L: %f R: %f \n\r", aeL, aeR);
+        pc.printf("cSP  L: %f R: %f \r\n\n", cSetL, cSetR);
+        //Var_Lock.unlock();
         
         /*if (pc.readable()){
             x=pc.getc();
@@ -147,14 +146,14 @@
         //float eR = 0;
         const unsigned short maxError = 1000;
         const unsigned short maxAcc = 10000;
-        const float Kp = 1.2;
-        const float Ki = 1.2;
+        const float Kp = 0.1f;
+        const float Ki = 0.05f;
         
         prevu1 = u1;
         prevu2 = u2;
         
         // read encoder and calculate speed of both motors
-        GetSpeeds(&fbSpeedL, &fbSpeedR);
+        GetSpeeds();
         
         // calculate error
         eL = userSetL - fbSpeedL;
@@ -180,7 +179,7 @@
         {
             eR = -maxError;
         }
-        */
+        */   
         
         // accumulated error (integration)
         if (prevu1 < 1 && prevu1 > -1)
@@ -192,6 +191,9 @@
             aeR += eR;
         }
         
+        //aeL += eL;
+        //aeR += eR;
+        
         // bound the accumulatd error
         /*
         if (aeL > maxAcc)
@@ -215,7 +217,11 @@
         u1 = Kp*eL + Ki*aeL;
         u2 = Kp*eR + Ki*aeR;
         
+        cSetL = userSetL + u1;
+        cSetR = userSetR + u2;
         
+        //u1 = -u1;
+        //u2 = -u2;
         // Is signaled by a periodic timer interrupt handler
         /*
         Read incremental position, dPosition, and time interval from the QEI.
@@ -226,8 +232,8 @@
         Update the DIR pin on the LMD18200 with the sign of u.
         */
         
-        SetLeftMotorSpeed(u1);
-        SetRightMotorSpeed(u2);
+        SetLeftMotorSpeed(cSetL);
+        SetRightMotorSpeed(cSetR);
     } 
 }
 
@@ -367,10 +373,10 @@
     }
 }
 
-void GetSpeeds(float *leftSpeed, float *rightSpeed)
+void GetSpeeds()
 {
-    float leftMaxPos = 1438.0f;
-    float rightMaxPos = 1484.0f;
+    float leftMaxPos = 1480.0f;
+    float rightMaxPos = 1480.0f;
     
     // Restart the SPI module each time
     SpiStart = 1;
@@ -385,6 +391,7 @@
     dTimeRight = DE0.write(Dummy);      // Read QEI-1 time interval register
     
     // figure out which direction the motor is turning
+    /*
     if (dPositionLeft > 32767)
     {
         // turning backwards
@@ -406,23 +413,28 @@
         // turning forwards
         *rightSpeed = dPositionRight/rightMaxPos;
     }
+    */
     Var_Lock.unlock();
     
+    // calcspeed
+    fbSpeedL = ((float)dPositionLeft)/leftMaxPos;
+    fbSpeedR = ((float)dPositionRight)/rightMaxPos;
+    
     // bound the feedback speed
-    if (*leftSpeed > 1)
+    if (fbSpeedL > 1)
     {
-        *leftSpeed = 1;
+        fbSpeedL = 1;
     }
-    if (*leftSpeed < -1)
+    if (fbSpeedL < -1)
     {
-        *leftSpeed = -1;
+        fbSpeedL = -1;
     }
-    if (*rightSpeed > 1)
+    if (fbSpeedR > 1)
     {
-        *rightSpeed = 1;
+        fbSpeedR = 1;
     }
-    if (*rightSpeed < -1)
+    if (fbSpeedR < -1)
     {
-        *rightSpeed = -1;
+        fbSpeedR = -1;
     }
 } 
\ No newline at end of file