Diff: main.cpp

Revision:

2:eb4101b574bc

Parent:

1:fec1d091fa34

--- a/main.cpp	Wed Mar 06 13:58:03 2013 +0000
+++ b/main.cpp	Wed Mar 06 19:20:02 2013 +0000
@@ -45,6 +45,10 @@
 float turnradius;
 float wratio_desired;                                                   //Right/Left
 
+//Opeloop Flag
+volatile bool openloop_lowrpm = true;
+const float rpm_openloop_limit = 100;
+
 //PID constants
 float kp=0;
 float kd=0;
@@ -205,10 +209,18 @@
         Prev_time_Left = Current_time_Left;                             // Set Current Reading as previous reading for next iteration
         cnt_Left = 0;                                                   //Reset Count
         rpm_Left = 1 / ( time_elapsed_Left  / (1000000*60) );           //Evaluate RPM from time for 1 revolution
+        if (rpm_Left < rpm_openloop_limit)
+        {
+            openloop_lowrpm = true;
+        }
+        else
+        {
+            openloop_lowrpm = false;
+        }
         displayLeftrpm = true;                                          //Display RPM - for testing purposes only
     }
     
-    if (MissingTeethCheckLeft)                                          //Called every 10 sec
+    if (MissingTeethCheckLeft && !openloop_lowrpm)                                          //Called every 10 sec
     {
          countLeftMissingTeeth++;        
          if (countLeftMissingTeeth == 1)                                //No comparisons for first tooth ( 12th is compared with first)
@@ -258,10 +270,14 @@
         Prev_time_Right = Current_time_Right;                             // Set Current Reading as previous reading for next iteration
         cnt_Right = 0;                                                    //Reset Count
         rpm_Right = 1 / ( time_elapsed_Right  / (1000000*60) );           //Evaluate RPM from time for 1 revolution
+         if (rpm_Right < rpm_openloop_limit)
+        {
+            openloop_lowrpm = true;
+        }
         displayRightrpm = true;                                           //Evaluate RPM from time for 1 revolution
      }
     
-    if (MissingTeethCheckRight)                                           //Called every 10 sec
+    if (MissingTeethCheckRight && !openloop_lowrpm)                                           //Called every 10 sec
     {
          countRightMissingTeeth++;        
          if (countRightMissingTeeth == 1)                                 //No comparisons for first tooth ( 12th is compared with first)