Diff: main.cpp

Revision:

6:5200ce9fa5a7

Parent:

5:7108ac9e8182

Child:

7:751d5e3e9cab

--- a/main.cpp	Mon Feb 25 21:07:42 2013 +0000
+++ b/main.cpp	Tue Feb 26 19:58:06 2013 +0000
@@ -7,9 +7,12 @@
 
 // --- Constants
 #define Dummy 0
-#define PWMPeriod 0.0005 // orignally 0.001
+
+//#define PWMPeriod 0.0005 // orignally 0.001
+const float PWMPeriod = 0.0005;
 // 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 ControlUpdate 0.05
+const float ControlUpdate = 0.05;
 #define EncoderTime 610
 
 
@@ -24,25 +27,25 @@
 void InitializeEncoder();
 void InitializePWM();
 void PwmSetOut(float d, float T);
-void ReadEncoder();
+void GetSpeeds(float *leftSpeed, float *rightSpeed);
 void SetLeftMotorSpeed(float u);
 void SetRightMotorSpeed(float u);
-Mutex Var_Lock;
 
-// Global variables for interrupt handler
+// Global variables
 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;
+Mutex Var_Lock;
+
+// global variables to eventually be removed
+unsigned int dPositionLeft, dTimeLeft, dPositionRight, dTimeRight;
+float fbSpeedL, fbSpeedR;
 float eL = 0;
 float eR = 0;
-float fbSpeedL;
-float fbSpeedR;
 
 // --- Processes and threads
 int32_t SignalPi, SignalWdt, SignalExtCollision;
@@ -99,8 +102,6 @@
     
     BluetoothSerial.printf("\n\n\rTap w-a-s-d keys for differential speed control: ");
     
-    char c;
-    
     while(1)
     {
         Var_Lock.lock();
@@ -127,34 +128,9 @@
             pc.scanf("%f", &userSetL);
             pc.printf("%f", userSetL);
             userSetR = userSetL;
-            
-            /* x=pc.getc();
-            if(x=='w')
-            {
-                // increase motor speed
-                u1 += 0.02;
-                if (u1 > 1)
-                {
-                    u1 = 1;
-                }
-            }
-            else if(x=='s')
-            {
-                // u1ecrease motor speed
-                u1 -= 0.02;
-                if (u1 < 0)
-                {
-                    u1 = 0;
-                }
-            }
-            //else if(x=='a') ...
-            //else if(x=='d') ... 
-            */
         }
             
         Thread::wait(2000); // Wait 2 seconds
-        
-
     }
 }
 
@@ -166,35 +142,26 @@
         osSignalWait(SignalPi, osWaitForever); 
         led2= !led2; // Alive status
         
-        // Read encoder and display results
-        ReadEncoder();
-        
-        //float fbSpeedL;
-        //float fbSpeedR;
+        float prevu1, prevu2;
         //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;
+        const unsigned short maxError = 1000;
+        const unsigned short maxAcc = 10000;
+        const float Kp = 1.2;
+        const float Ki = 1.2;
         
         prevu1 = u1;
         prevu2 = u2;
         
-        // calculate feedback speed percentage
-        // ****** TODO : BOUND FEEDABCK SPEED
-        fbSpeedL = (leftPos/leftMaxPos); 
-        fbSpeedR = (rightPos/rightMaxPos);
+        // read encoder and calculate speed of both motors
+        GetSpeeds(&fbSpeedL, &fbSpeedR);
         
         // calculate error
         eL = userSetL - fbSpeedL;
         eR = userSetR - fbSpeedR;
         //eL = -eL;
         //eR = -eR;
+        
         // prevent overflow / bound the error
         /*
         if (eL > maxError)
@@ -259,14 +226,6 @@
         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);
     } 
@@ -315,12 +274,14 @@
     osTimerId OneShot = osTimerCreate(osTimer(Wdtimer), osTimerOnce, (void *)0);
     PeriodicInt.attach(&PiControllerISR, ControlUpdate); // Specify address of the TimerISR (Ticker) function and the interval between interrupts
     
+    InitializePWM();
     InitializeEncoder();
 }
 
 void InitializePWM()
 {
-
+    PwmL.period(PWMPeriod);
+    PwmR.period(PWMPeriod);
 }
 
 void InitializeEncoder()
@@ -360,7 +321,6 @@
     d = abs(u);
     onTime = d * T; 
 
-    PwmL.period(T);
     PwmL.pulsewidth(onTime);
     
     if (u > 0)
@@ -395,7 +355,6 @@
     d = abs(u);
     onTime = d * T; 
 
-    PwmR.period(T);
     PwmR.pulsewidth(onTime);
     
     if (u > 0)
@@ -408,46 +367,62 @@
     }
 }
 
-void ReadEncoder()
+void GetSpeeds(float *leftSpeed, float *rightSpeed)
 {
-    //int dPositionRight, dTimeRight, dPositionLeft, dTimeLeft;
+    float leftMaxPos = 1438.0f;
+    float rightMaxPos = 1484.0f;
     
-    
-    // May be executed in a loop
-    
+    // Restart the SPI module each time
     SpiStart = 1;
     wait_us(5);
     SpiStart = 0;
     DE0.write(0x8004);
-    
+    // read in 4 16-bit words
     Var_Lock.lock();
-    dPositionLeft = DE0.write(Dummy); // Read QEI-0 position register 
-    dTimeLeft = DE0.write(Dummy);     // Read QE-0 time interval register
+    dPositionLeft = DE0.write(Dummy);   // Read QEI-0 position register 
+    dTimeLeft = DE0.write(Dummy);       // Read QE-0 time interval register
     dPositionRight = DE0.write(Dummy);  // Read QEI-1 position register 
     dTimeRight = DE0.write(Dummy);      // Read QEI-1 time interval register
-    Var_Lock.unlock();
     
-    // check for bad values
-    /*
-    if (startup >= 10)
+    // figure out which direction the motor is turning
+    if (dPositionLeft > 32767)
     {
-        if (dTimeRight > (EncoderTime + 5) || dTimeRight < (EncoderTime - 5) || dTimeLeft > (EncoderTime + 5) || dTimeLeft < (EncoderTime - 5))
-        {
-            // Failure!!
-            u1 = 0;
-            pc.printf("DEO FAILURE!! \n\r\n");
-        }
+        // turning backwards
+        *leftSpeed = -(65535 - dPositionLeft)/leftMaxPos;
     }
     else
     {
-        startup += 1;
+        // turning forwards
+        *leftSpeed = dPositionLeft/leftMaxPos;
     }
-    */
+    
+    if (dPositionRight > 32767)
+    {
+        // turning backwards
+        *rightSpeed = -(65535 - dPositionRight)/rightMaxPos;
+    }
+    else
+    {
+        // turning forwards
+        *rightSpeed = dPositionRight/rightMaxPos;
+    }
+    Var_Lock.unlock();
     
-    /*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);*/
-    
-    // simply write out the results for now
-}  
\ No newline at end of file
+    // bound the feedback speed
+    if (*leftSpeed > 1)
+    {
+        *leftSpeed = 1;
+    }
+    if (*leftSpeed < -1)
+    {
+        *leftSpeed = -1;
+    }
+    if (*rightSpeed > 1)
+    {
+        *rightSpeed = 1;
+    }
+    if (*rightSpeed < -1)
+    {
+        *rightSpeed = -1;
+    }
+} 
\ No newline at end of file