Diff: main.cpp

Revision:

23:806c9b8af77c

Parent:

22:3cda0d788452

Child:

24:f4f933d9194b

--- a/main.cpp	Wed Apr 10 13:16:29 2013 +0000
+++ b/main.cpp	Sat Apr 13 19:19:20 2013 +0000
@@ -51,17 +51,17 @@
 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
+float RecV[200]; // Record Feedback Speed
+float RecU[200]; // Record Motor Input
+float RecX[200]; // Record Integrator Output
+float RecE[200]; // Record Error
 int RecCount = 0; // Record Counter
 unsigned short action;
 float DF = 0;
 float DR = 0;
 float TSpeedL = 0;
 float TSpeedR = 0;
-float Turn = 0.075;
+float Turn = 0.1;
 float aeW = 0;
 float eW = 0;
 float uW = 0; 
@@ -69,10 +69,10 @@
 
 float Nominal = 35;
 float Kpos = 0.01;
-float Kor = 0.005;
-float KpW = 0.4;
+float Kor = 0.01;
+float KpW = 0.3;
 float KiW = 0.0;
-
+int autoSpeed = -1;
 
 Mutex Var_Lock;
 
@@ -173,10 +173,10 @@
                         BtS.printf("\n\r Welcome to Wall Following, use w-s to control robot speed");
                         BtS.printf("\n\r Use a-d to increase/decrease turning radius");
                         BtS.printf("\n\r Use < and > to increase/decrease wall distance");
-                        userSetL = 0.1;
-                        userSetR = 0.1;
-                        TSpeedL = 0.1;
-                        TSpeedR = 0.1;
+                        userSetL = 0.2;
+                        userSetR = 0.2;
+                        TSpeedL = 0.2;
+                        TSpeedR = 0.2;
                         action = 2;
                         break;
                     case '0':
@@ -201,12 +201,12 @@
                 switch(c)
                 {
                     case('w'):
-                        userSetL = userSetL + 0.08;
-                        userSetR = userSetR + 0.08;
+                        userSetL = userSetL + 0.1;
+                        userSetR = userSetR + 0.1;
                         break;
                     case('s'):
-                        userSetL = userSetL - 0.08;
-                        userSetR = userSetR - 0.08;
+                        userSetL = userSetL - 0.1;
+                        userSetR = userSetR - 0.1;
                         break;
                     case('a'):
                         userSetL = userSetL - 0.04;
@@ -217,7 +217,7 @@
                         userSetR = userSetR - 0.04;
                         break;          
                     case('e'):
-                        Ramp(0.4, 500, 0);
+                        Ramp(0.5, 500, 0);
                         break;
                     case('r'):
                         Ramp(0, 500, 0);
@@ -257,10 +257,10 @@
                         TSpeedR = TSpeedR - 0.05;
                         break;
                     case('a'):
-                        Turn = Turn + 0.005;
+                        Turn = Turn + 0.01;
                         break;
                     case('d'):
-                        Turn = Turn - 0.005;
+                        Turn = Turn - 0.01;
                         break;
                     case(','):
                         Nominal = Nominal - 2.5;
@@ -268,6 +268,9 @@
                     case('.'):
                         Nominal = Nominal + 2.5;
                         break; 
+                    case('g'):
+                        autoSpeed = autoSpeed * -1;
+                        break;
                     case('n'):
                         BtS.printf("\n\r Current constants: Ki %.3f:, Kp: %.3f, Kor: %.3f, Kpos: %.3f \n\r Select the constant you wish to change:", KiW, KpW, Kor, Kpos);    
                         char k;
@@ -319,13 +322,13 @@
                 //userSetR = userSetL;
             }            
             
-            if (action == 4 && c == 'r')
+            if (action == 2 && c == 'r')
             {
-                if (RecCount == 100)
+                if (RecCount == 200)
                 {
                     BtS.printf("\n\n\rRecV    RecU    RecX    RecE \n\r");
                     int i = 0;
-                    for (i = 0; i < 100; i++)
+                    for (i = 0; i < 200; i++)
                     {
                         BtS.printf("%f, %f, %f, %f \n\r", RecV[i], RecU[i], RecX[i], RecE[i]);
                     }
@@ -364,48 +367,6 @@
             Var_Lock.unlock();
             Thread::wait(2000);
         }          
-
-/*
-    while(1)
-    {
-        Thread::wait(20);
-    
-
-        
-        
-
-        if(BtS.readable()) 
-        {
-            c = BtS.getc();
-            if (c == 'r')
-            {
-                userSetL = 0.2;
-            }
-            if (c == 'p')
-            {
-                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]);
-                    }
-                }    
-            
-            }
-            
-            if (c == 'n')
-            {
-                BtS.printf("\n\r Enter a motor speed (with sign as direction:\n\r");
-                BtS.scanf("%f", &userSetL);
-                
-                BtS.printf("%f", userSetL);
-                userSetR = userSetL;
-            }    
-        }
-            
-        Thread::wait(2000);   Wait 2 seconds */
     }
 }
 
@@ -425,6 +386,7 @@
         SetLeftMotorSpeed(userSetL);
         SetRightMotorSpeed(userSetR);
         BtS.printf("\n\rEmergency Stop!!\n\r");
+        Thread::wait(2000);
         }
         
         osSignalWait(SignalPi, osWaitForever); 
@@ -502,19 +464,6 @@
         u1 = Kp*eL + Ki*aeL;
         u2 = Kp*eR + Ki*aeR;
         
-        // data recording code
-        if (userSetL == 0.2f || action == 4)
-        {
-            if (RecCount < 100)
-            {
-                RecX[RecCount] = aeL;
-                RecU[RecCount] = u1;
-                RecV[RecCount] = fbSpeedL;
-                RecE[RecCount] = eL;
-                RecCount++;
-            }
-        }
-        
         SetLeftMotorSpeed(u1);
         SetRightMotorSpeed(u2);
     } 
@@ -719,6 +668,7 @@
 void IRChecker()
 {
     float IRF, IRR, IRF1, IRR1, IRF2, IRR2, prevDF, prevDR;
+    float speedSet;
     float aF = 22.6321; // 34.2911
     float bF = -0.1721; // -0.2608
     float aR = 22.6021; // 34.2456
@@ -789,9 +739,40 @@
         uW = -Turn;
     }
     
-    // set differential speeds
-    userSetL = TSpeedL + uW;
-    userSetR = TSpeedR - uW;   
+    // set speed using auto speed control
+    if (autoSpeed == 1)
+    {
+        speedSet = (1 - (abs(eW)*5))*TSpeedL;
+        if (speedSet < 0.05)
+        {
+            speedSet = 0.05;
+        }
+        userSetL = speedSet + uW;
+        userSetR = speedSet - uW;   
+    }
+    else
+    {
+        // set differential speeds
+        userSetL = TSpeedL + uW;
+        userSetR = TSpeedR - uW;
+    }   
+    
+    
+    // data recording code
+        if (action == 2)
+        {
+            if (RecCount < 200)
+            {
+                RecX[RecCount] = eW;
+                RecU[RecCount] = uW;
+                RecV[RecCount] = DF;
+                RecE[RecCount] = DR;
+                RecCount++;
+                //
+            }
+        }
+    
+    
 }
 
 /*