Diff: scanner.cpp

Revision:

6:5e24ff86b743

Parent:

5:4d5601a9dffe

--- a/scanner.cpp	Tue Mar 22 18:11:36 2016 +0000
+++ b/scanner.cpp	Fri Mar 25 19:51:11 2016 +0000
@@ -23,21 +23,17 @@
     servoL.period(1.0/50.0);
     servoR.period(1.0/50.0);
 
-    float temp[13] = {0.0500, 0.0542, 0.0583, 0.0625, 0.0667, 0.0708,
-        0.0750, 0.0792, 0.0833, 0.0875, 0.0917, 0.0958,
-        0.1000};
+    float temp[7] = {0.0575, 0.0663, 0.0750, 0.0837, 0.0925,
+    0.1013, 0.1100}; // sub-micro
 
-    for (int i = 0; i < 13; i++)
+    for (int i = 0; i < 7; i++)
         DUTY[i] = temp[i];
-        // 0.05 = min duty cycle
-        // {0.0500, 0.0542, 0.0583, 0.0625, 0.0667, 0.0708,
-        // 0.0750, 0.0792, 0.0833, 0.0875, 0.0917, 0.0958,
-        // 0.1000};
 
     obstacle = 0;
     distLeft = 0.0;
     distRight = 0.0;
-    distForward = 0.0;
+    distForwardL = 0.0;
+    distForwardR = 0.0;
 
     // initializing to avoid status
     avoidFlag = 1;
@@ -51,6 +47,7 @@
     wait(0.2);
     pitEnable = 1;
     scanPit.attach(this, &Scanner::scan, period);
+pc.printf("Scanner created\n");
 }
 
 // FUNCTION:
@@ -67,8 +64,8 @@
     huntFlag = 1;
     invertL = -1;
     invertR = 1;
-    dutyL = HALF_DUTY;
-    dutyR = HALF_DUTY;
+    dutyL = MAX_DUTY;
+    dutyR = MIN_DUTY;
     servoL.write(DUTY[dutyL]);
     servoR.write(DUTY[dutyR]);
     wait(0.1);
@@ -116,16 +113,83 @@
     wait(0.1);
     distLeft = longRangeL.distInchesL();
     distRight = longRangeR.distInchesR();
-    dutyL = HALF_DUTY;
-    dutyR = HALF_DUTY;
+    dutyL = MAX_DUTY;
+    dutyR = MIN_DUTY;
     servoL.write(DUTY[dutyL]);
     servoR.write(DUTY[dutyR]);
     wait(0.1);
-    distForward = ( longRangeL.distInchesL() + longRangeR.distInchesR() )/2.0;
+    distForwardL = longRangeL.distInchesL();
+    distForwardR = longRangeR.distInchesR();
     pitEnable = 1;
 }
 
 // FUNCTION:
+//      void localizeLeft()
+// IN-PARAMETERS:
+//      None
+// OUT-PARAMETERS:
+//      None
+// DESCRIPTION:
+//      Using sensor longRangeR this method will localize to the wall
+//      to the left during stretches of forward motion where robot 
+//      should remain xxx distance from the left wall. 
+void Scanner::localizeLeft()
+{
+    /****** pseudocode *****
+    
+    dist = longRangeR dist measure
+    
+    if (dist > xxx dist)
+        pitEnable = 0
+        pause current route
+        adjust angle to left
+        unpause route
+        pitEnable = 1
+        
+        
+    else if (dist < xxx dist)
+        pitEnable = 0
+        pause current route
+        adjust angle to right
+        unpause route
+        pitEnable = 1
+    */
+}
+
+// FUNCTION:
+//      void localizeRight()
+// IN-PARAMETERS:
+//      None
+// OUT-PARAMETERS:
+//      None
+// DESCRIPTION:
+//      Using sensor longRangeL this method will localize to the wall
+//      to the left during stretches of forward motion where robot 
+//      should remain xxx distance from the left wall. This function
+//      will be called from scan when the localizeRightFlag is set.
+void localizeRight()
+{
+    /****** pseudocode *****
+    
+    dist = longRangeL dist measure
+    
+    if (dist > xxx dist)
+        pitEnable = 0
+        pause current route
+        adjust angle to right
+        unpause route
+        pitEnable = 1
+        
+    else if (dist < xxx dist)
+        pitEnable = 0
+        pause current route
+        adjust angle to left
+        unpause route
+        pitEnable = 1
+    */
+}
+
+// FUNCTION:
 //      scan()
 // IN-PARAMETERS:
 //      None
@@ -140,10 +204,12 @@
 {
     if (pitEnable == 1)
     {
-        // obtain distance measures from sensors
+        /*// obtain distance measures from sensors
         distLeft = longRangeL.distInchesL();
         distRight = longRangeR.distInchesR();
         
+        //check localize flags and make necessary method calls
+        
         // check avoid flag
         if (avoidFlag == 1)
         {
@@ -151,18 +217,17 @@
         }
         
         // check hunt flag
-        else if (huntFlag == 1)
+        if (huntFlag == 1)
         {
             // add hunt code
-        }
+        }*/
 
         // increment/decrement servo position
         dutyL += invertL;
         dutyR += invertR;
         servoL.write(DUTY[dutyL]);
         servoR.write(DUTY[dutyR]);
-        if(((dutyL == HALF_DUTY) && (dutyR == HALF_DUTY)) ||
-            ((dutyL == MIN_DUTY) && (dutyR == MAX_DUTY)) ||
+        if(((dutyL == MIN_DUTY) && (dutyR == MAX_DUTY)) ||
             ((dutyL == MAX_DUTY) && (dutyR == MIN_DUTY)))
         {
             invertL *= -1;