Diff: main.cpp

Revision:

4:03bf5bdca9fb

Parent:

3:9a39e487b724

Child:

5:7108ac9e8182

--- a/main.cpp	Fri Feb 15 21:11:34 2013 +0000
+++ b/main.cpp	Fri Feb 22 20:57:43 2013 +0000
@@ -7,7 +7,11 @@
 
 // --- Constants
 #define Dummy 0
-#define PWMPeriod 0.001
+#define PWMPeriod 0.0005 // orignally 0.001
+#define ControlUpdate 0.05
+#define EncoderTime 610
+#define Kp = 1.2;
+#define Ki = 1.2;
 
 // --- Function prototypes
 void PiControllerISR(void);
@@ -23,11 +27,15 @@
 void ReadEncoder();
 void SetLeftMotorSpeed(float u);
 void SetRightMotorSpeed(float u);
+Mutex Var_Lock;
 
 // Global variables for interrupt handler
 float u1;
 float u2;
 int dPositionRight, dTimeRight, dPositionLeft, dTimeLeft;
+int startup = 0;
+float aeL = 0; 
+float aeR = 0;
 
 // --- Processes and threads
 int32_t SignalPi, SignalWdt, SignalExtCollision;
@@ -46,12 +54,12 @@
 
 Bluetooth tx    13|-|28
 Bluetooth rx    14|-|27
-                15|-|26 Brake, Right Motor, M1
-                16|-|25 Dir, Right Motor, M1
-                17|-|24 PWM, Right Motor, M1
-                18|-|23 Brake, Left Motor, M2
-                19|-|22 Dir, Left Motor, M2
-                20|-|21 PWM, Left Motor, M2
+                15|-|26 Brake, Left Motor, M1
+                16|-|25 Dir, Left Motor, M1
+                17|-|24 PWM, Left Motor, M1
+                18|-|23 Brake, Right Motor, M2
+                19|-|22 Dir, Right Motor, M2
+                20|-|21 PWM, Right Motor, M2
 */
 
 // --- IO Port Configuration
@@ -84,9 +92,17 @@
     
     BluetoothSerial.printf("\n\n\rTap w-a-s-d keys for differential speed control: ");
     
+    char c;
+    
     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\n\r", dTimeRight);
+        Var_Lock.unlock();
+        
         /*if (pc.readable()){
             x=pc.getc();
             pc.putc(x); //Echo keyboard entry
@@ -98,6 +114,10 @@
             pc.printf("\n\r Enter a motor speed (with sign as direction:\n\r");
             pc.scanf("%f", &u1);
             pc.printf("%f", u1);
+            u2 = u1;
+            
+            
+            
             /* x=pc.getc();
             if(x=='w')
             {
@@ -122,7 +142,7 @@
             */
         }
             
-        Thread::wait(500); // Wait 500 ms
+        Thread::wait(2000); // Wait 2 seconds
         
 
     }
@@ -139,8 +159,37 @@
         // Read encoder and display results
         ReadEncoder();
         
+        float fbSpeedL;
+        float fbSpeedR;
+        float eL = 0;
+        float eR = 0;
+        
+        // calculate feedback speed percentage
+        fbSpeedL = dPositionLeft/1438; 
+        fbSpeedR = dPositionRight/1484;
+        
+        // calculate error
+        eL = u1 - fbSpeedL;
+        eR = u2 - fbSpeedR;
+        
+        // accumulated error (integration)
+        aeL += eL;
+        aeR += eR;
+        
+        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.
+        e = Setpoint – dPosition // e is the velocity error
+        xState = xState + e;    // x is the Euler approximation to the integral of e.
+        u = Kp*e + Ki*xState;       // u is the control signal
+        Update PWM on-time register with abs(u);
+        Update the DIR pin on the LMD18200 with the sign of u.
+        */
+        
         SetLeftMotorSpeed(u1);
-        SetRightMotorSpeed(u1);
+        SetRightMotorSpeed(u2);
     } 
 }
 
@@ -185,7 +234,7 @@
     PiControl = osThreadCreate(osThread(PiControlThread), NULL);
     ExtCollision = osThreadCreate(osThread(ExtCollisionThread), NULL);
     osTimerId OneShot = osTimerCreate(osTimer(Wdtimer), osTimerOnce, (void *)0);
-    PeriodicInt.attach(&PiControllerISR, 1); // Specify address of the TimerISR (Ticker) function and the interval between interrupts
+    PeriodicInt.attach(&PiControllerISR, ControlUpdate); // Specify address of the TimerISR (Ticker) function and the interval between interrupts
     
     InitializeEncoder();
 }
@@ -199,6 +248,7 @@
 {
     // Initialization – to be executed once (normally)
     DE0.format(16,0);   // SPI format: 16-bit words, mode 0 protocol.
+    DE0.frequency(1000000);
     SpiStart = 0;
     SpiReset = 1;
     wait_us(10);
@@ -283,17 +333,42 @@
 {
     //int dPositionRight, dTimeRight, dPositionLeft, dTimeLeft;
     
+    
     // May be executed in a loop
-    dPositionRight = DE0.write(Dummy); // Read QEI-0 position register 
-    dTimeRight = DE0.write(Dummy);     // Read QE-0 time interval register
-    dPositionLeft = DE0.write(Dummy);  // Read QEI-1 position register 
-    dTimeLeft = DE0.write(Dummy);      // Read QEI-1 time interval register
+    
+    SpiStart = 1;
+    wait_us(5);
+    SpiStart = 0;
+    DE0.write(0x8004);
+    
+    Var_Lock.lock();
+    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)
+    {
+        if (dTimeRight > (EncoderTime + 5) || dTimeRight < (EncoderTime - 5) || dTimeLeft > (EncoderTime + 5) || dTimeLeft < (EncoderTime - 5))
+        {
+            // Failure!!
+            u1 = 0;
+            pc.printf("DEO FAILURE!! \n\r\n");
+        }
+    }
+    else
+    {
+        startup += 1;
+    }
+    */
+    
+    /*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
-    
-    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);
-    
 }  
\ No newline at end of file