Revision 17:bc13550f673b, committed 2013-03-20

Comitter:

IanTheMBEDMaster

Date:

Wed Mar 20 01:42:03 2013 +0000

Parent:

16:cf9519c35510

Child:

18:4b3ad79d1068

Commit message:

Played with PI

Changed in this revision

--- a/main.cpp	Tue Mar 19 23:16:21 2013 +0000
+++ b/main.cpp	Wed Mar 20 01:42:03 2013 +0000
@@ -63,8 +63,10 @@
 float TSpeedR = 0;
 float Turn = 0.01;
 float aeW = 0;
-float eW;
-float uW;
+float eW = 0;
+float uW = 0; 
+float prevuW = 0;
+
 
 Mutex Var_Lock;
 
@@ -168,6 +170,10 @@
                     case '0':
                         action = 3;
                         break;
+                    case 'r':
+                        action = 4;
+                        
+                        break;
                     case 'q':
                         action = 0;
                         break;
@@ -233,30 +239,44 @@
                 BtS.scanf("%f", &newSpeed);
                 
                 BtS.printf("%f", newSpeed);
-                Ramp(newSpeed, 1000, 0);
+                Ramp(newSpeed, 20, 0);
                 //userSetR = userSetL;
             }            
+            
+            if (action == 4 && c == 'r')
+            {
+                if (RecCount == 100)
+                {
+                    BtS.printf("\n\n\rRecV    RecU    RecX    RecE \n\r");
+                    int i = 0;
+                    for (i = 0; i < 100; i++)
+                    {
+                        BtS.printf("%f, %f, %f, %f \n\r", RecV[i], RecU[i], RecX[i], RecE[i]);
+                    }
+                }
+            }     
         }// close if(BtS.readable())
         if (action == 2)
         {
-            Var_Lock.lock();
-            BtS.printf("IR   F: %f cm R: %f cm \n\r", DF, DR);
-            BtS.printf("Wall Error: %f \n\r", eW);
-            BtS.printf("Acc Error: %f \n\r", aeW);
-            BtS.printf("Diff. Setpoint: %f \n\r", uW);
-            BtS.printf("Setpoint L: %f R: %f \n\n\r", userSetL, userSetR);
-            Var_Lock.unlock();
-            Thread::wait(1000);
+            // Wall Following
+            //Var_Lock.lock();
+            //BtS.printf("IR F: %f cm R: %f cm \n\r", DF, DR);
+//            BtS.printf("Wall Error: %f \n\r", eW);
+//            BtS.printf("Acc Error: %f \n\r", aeW);
+            //BtS.printf("Diff. Setpoint: %f \n\r", uW);
+//            BtS.printf("Setpoint L: %f R: %f \n\n\r", userSetL, userSetR);
+            //Var_Lock.unlock();
+            //Thread::wait(1000);
         }
         
         if (action == 3)
         {
-            // display debug messages
+            // Debug Mode
             
             float IRF, IRR;
             IRF = IRFront.read();
             IRR = IRRear.read();
-                  
+            
             Var_Lock.lock();
             BtS.printf("Pos. L: %d R: %d \n\r", dPositionLeft, dPositionRight);
             BtS.printf("Time L: %d R: %d \n\r", dTimeLeft, dTimeRight);
@@ -267,8 +287,7 @@
             BtS.printf("IR   F: %f R: %f \n\r", IRF, IRR);
             Var_Lock.unlock();
             Thread::wait(2000);
-        }
-          
+        }          
 
 /*
     while(1)
@@ -330,10 +349,10 @@
         //float eR = 0;
 
         // Kp = 0.1, Ki = 0.5
-        const float Kp = 0.1f;
-        const float Ki = 0.5f;
+        const float Kp = 0.3f;
+        const float Ki = 0.8f;
         
-        if (action == 2)
+        if (action == 2 || action == 4)
         {
             IRChecker();
         }
@@ -399,29 +418,21 @@
         u1 = Kp*eL + Ki*aeL;
         u2 = Kp*eR + Ki*aeR;
         
-        cSetL = userSetL + u1;
-        cSetR = userSetR + u2;
-        
         // data recording code
-        /*if (userSetL == 0.8f)
+        if (userSetL == 0.2f || action == 4)
         {
             if (RecCount < 100)
             {
-                RecX[RecCount] = aeL;
-                RecU[RecCount] = cSetL;
-                RecV[RecCount] = fbSpeedL;
-                RecE[RecCount] = eL;
+                RecX[RecCount] = DF;
+                RecU[RecCount] = DR;
+                RecV[RecCount] = eW;
+                RecE[RecCount] = uW;
                 RecCount++;
             }
-            else
-            {
-                userSetL = 0;
-            }
-          
-        }*/
+        }
         
-        SetLeftMotorSpeed(cSetL);
-        SetRightMotorSpeed(cSetR);
+        SetLeftMotorSpeed(u1);
+        SetRightMotorSpeed(u2);
     } 
 }
 
@@ -620,34 +631,39 @@
 
 void IRChecker()
 {
-    float IRF, IRR, prevDF, prevDR;
+    float IRF, IRR, IRF1, IRR1, IRF2, IRR2, prevDF, prevDR;
     float aF = 22.6321; // 34.2911
     float bF = -0.1721; // -0.2608
     float aR = 22.6021; // 34.2456
     float bR = -0.0376; // -0.0569
     float Nominal = 25;
-    float Kpos = 0.001;
-    float Kor = 0.001;
+    float Kpos = 0.02;
+    float Kor = 0.01;
     float KpW = 0.1;
     float KiW = 0.4;
     
     // Read Sensors
-    IRF = 3.3*IRFront.read();
-    IRR = 3.3*IRRear.read();
-
+    IRF1 = 3.3*IRFront.read();
+    IRR1 = 3.3*IRRear.read();
+    IRF2 = 3.3*IRFront.read();
+    IRR2 = 3.3*IRRear.read();
+    
+    // average
+    IRF = (IRF1 + IRF2)/2;
+    IRR = (IRR1 + IRR2)/2;
+    
     // Calculate distance based on voltage
     prevDF = DF;
     prevDR = DR;
     DF = aF/(IRF+bF);
     DR = aR/(IRR+bR);
     
+    prevuW = uW;
+    
     // check for invalid data
-    if ((DR - prevDR) > 5 || (prevDR - DR) > 5)
-    {
-        DR = prevDR 
-    
-    
-    
+    //if ((DR - prevDR) > 5 || (prevDR - DR) > 5)
+    //{
+    //    DR = prevDR 
     
     //
     
@@ -655,17 +671,20 @@
     eW = Kpos*(Nominal - (DF + DR)/2) + Kor*(DR - DF);
     
     // accumulate error
-    aeW = aeW + eW;
+    if (prevuW != 0.025 || prevuW != -0.025)
+    {
+        aeW = aeW + eW;
+    }
     
     uW = KpW*eW + KiW*aeW;
     
-    if (uW > 0.02)
+    if (uW > 0.05)
     {
-        uW = 0.02;
+        uW = 0.05;
     }
-    else if (uW < -0.02)
+    else if (uW < -0.05)
     {
-        uW = -0.02;
+        uW = -0.05;
     }
     
     // set differential speeds
@@ -736,6 +755,7 @@
     BtS.printf("|  d  | Drive the robot using wasd keys\n\r");
     BtS.printf("|  w  | Robot performs wall following\n\r");
     BtS.printf("|  0  | Debug mode\n\r");
+    BtS.printf("|  r  | Record Data \n\r");
     BtS.printf("|  q  | Quit current action, stop the robot, and return to this menu\n\r\n");   
 }