Diff: main.cpp

Revision:

19:b9c4952f4acc

Parent:

18:4b3ad79d1068

Child:

20:344c088d3ead

--- a/main.cpp	Fri Mar 22 19:57:55 2013 +0000
+++ b/main.cpp	Fri Apr 05 17:37:35 2013 +0000
@@ -66,7 +66,12 @@
 float eW = 0;
 float uW = 0; 
 float prevuW = 0;
-float Nominal = 25;
+
+float Nominal = 35;
+float Kpos = 0.01;
+float Kor = 0.0;
+float KpW = 0.1;
+float KiW = 0.2;
 
 
 Mutex Var_Lock;
@@ -238,7 +243,44 @@
                         break;
                     case('.'):
                         Nominal = Nominal + 5;
-                        break;               
+                        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;
+                        float newConst;
+                        while (1)
+                        {
+                            if (BtS.readable())
+                            {
+                                k = BtS.getc();
+                                if (k == '1')
+                                {
+                                    BtS.scanf("%f", &newConst);
+                                    KiW = newConst;
+                                    break;
+                                }
+                                if (k == '2')
+                                {
+                                    BtS.scanf("%f", &newConst);
+                                    KpW = newConst;
+                                    break;
+                                }
+                                if (k == '3')
+                                {
+                                    BtS.scanf("%f", &newConst);
+                                    Kor = newConst;
+                                    break;
+                                }
+                                if (k == '4')
+                                {
+                                    BtS.scanf("%f", &newConst);
+                                    Kpos = newConst;
+                                    break;
+                                }
+                                
+                                printf("\n\r Pick a constant ya goof \n\r");
+                            }
+                        }
                 }
             }
             if (action == 3)
@@ -656,11 +698,6 @@
     float bF = -0.1721; // -0.2608
     float aR = 22.6021; // 34.2456
     float bR = -0.0376; // -0.0569
-    float Nominal = 35;
-    float Kpos = 0.1;
-    float Kor = 0.0;
-    float KpW = 0.01;
-    float KiW = 0.02;
     
     // Read Sensors
     IRF1 = 3.3*IRFront.read();
@@ -672,17 +709,27 @@
     IRF = (IRF1 + IRF2)/2;
     IRR = (IRR1 + IRR2)/2;
     
-    // Calculate distance based on voltage
     prevDF = DF;
     prevDR = DR;
     
-    
+    // Calculate distance based on voltage
     DF = aF/(IRF+bF);
     DR = aR/(IRR+bR);
     
     prevuW = uW;
     
     // check for invalid data
+    if (DF < 0)
+    {
+        DF = 80;
+    }
+    if (DR < 0)
+    {
+        DR = 80;
+    }
+    
+    // TODO: conditions when something is too close to the sensor
+    
     //if ((DR - prevDR) > 5 || (prevDR - DR) > 5)
     //{
     //    DR = prevDR