Diff: main.cpp

Revision:

8:d65cba3bfc0e

Parent:

7:751d5e3e9cab

Child:

9:0eb7b173d6c3

--- a/main.cpp	Wed Feb 27 21:46:23 2013 +0000
+++ b/main.cpp	Fri Mar 01 20:58:03 2013 +0000
@@ -41,6 +41,11 @@
 int startup = 0;
 float aeL = 0; 
 float aeR = 0;
+float RecV[100]; // Record Feedback Speed
+float RecU[100]; // Record Motor Input
+float RecX[100]; // Record Integrator Output
+float RecE[100]; // Record Error
+int RecCount = 0; // Record Counter
 Mutex Var_Lock;
 
 // global variables to eventually be removed
@@ -79,12 +84,12 @@
 DigitalOut led2(LED2);
 DigitalOut led3(LED3);
 DigitalOut led4(LED4);
-DigitalOut dirL(p22);
-DigitalOut brakeL(p23);
-PwmOut PwmL(p21);
-DigitalOut dirR(p25);
-DigitalOut brakeR(p26);
-PwmOut PwmR(p24);
+DigitalOut dirR(p22);
+DigitalOut brakeR(p23);
+PwmOut PwmR(p21);
+DigitalOut dirL(p25);
+DigitalOut brakeL(p26);
+PwmOut PwmL(p24);
 Serial BluetoothSerial(p13, p14); // (tx, rx) for PC serial channel
 Serial pc(USBTX, USBRX); // (tx, rx) for Parani/Promi Bluetooth serial channel
 SPI DE0(p5, p6, p7); // (mosi, miso, sclk) DE0 is the SPI channel with the DE0 FPGA 
@@ -106,6 +111,8 @@
     
     while(1)
     {
+        char c;
+    
         //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);
@@ -123,10 +130,33 @@
             }*/
         if(pc.readable()) 
         {
-            pc.printf("\n\r Enter a motor speed (with sign as direction:\n\r");
-            pc.scanf("%f", &userSetL);
-            pc.printf("%f", userSetL);
-            userSetR = userSetL;
+            c = pc.getc();
+            if (c == 'r')
+            {
+                userSetL = 0.2;
+            }
+            if (c == 'p')
+            {
+                if (RecCount == 100)
+                {
+                    pc.printf("\n\n\rRecV    RecU    RecX    RecE \n\r");
+                    int i = 0;
+                    for (i = 0; i < 100; i++)
+                    {
+                        pc.printf("%f, %f, %f, %f \n\r", RecV[i], RecU[i], RecX[i], RecE[i]);
+                    }
+                }    
+            
+            }
+            
+            if (c == 'n')
+            {
+                pc.printf("\n\r Enter a motor speed (with sign as direction:\n\r");
+                pc.scanf("%f", &userSetL);
+                
+                pc.printf("%f", userSetL);
+                userSetR = userSetL;
+            }    
         }
             
         Thread::wait(2000); // Wait 2 seconds
@@ -146,8 +176,9 @@
         //float eR = 0;
         const unsigned short maxError = 1000;
         const unsigned short maxAcc = 10000;
+        // Kp = 0.1, Ki = 0.5
         const float Kp = 0.1f;
-        const float Ki = 0.05f;
+        const float Ki = 0.5f;
         
         prevu1 = u1;
         prevu2 = u2;
@@ -231,7 +262,22 @@
         Update PWM on-time register with abs(u);
         Update the DIR pin on the LMD18200 with the sign of u.
         */
-        
+        if (userSetL == 0.2f)
+        {
+            if (RecCount < 100)
+            {
+                RecX[RecCount] = aeL;
+                RecU[RecCount] = cSetL;
+                RecV[RecCount] = fbSpeedL;
+                RecE[RecCount] = eL;
+                RecCount++;
+            }
+            else
+            {
+                userSetL = 0;
+            }
+           
+        }
         SetLeftMotorSpeed(cSetL);
         SetRightMotorSpeed(cSetR);
     }