Diff: scanner.cpp

Revision:

2:b281034eda86

Parent:

0:999bb8fcd0b2

Child:

3:992558a021d7

--- a/scanner.cpp	Wed Feb 17 16:47:55 2016 +0000
+++ b/scanner.cpp	Tue Feb 23 23:35:41 2016 +0000
@@ -6,71 +6,127 @@
 // FUNCTION:
 //      Scanner()
 // IN-PARAMETERS:
-//      
+//      Serial &pc1, PinName _servoL, PinName _servoR,
+//      VL6180x &_shortRangeL, VL6180x &_shortRangeR, Gp2x &_longRangeL,
+//      Gp2x &_longRangeR, float _period = 10e-3
 // OUT-PARAMETERS:
-//      
+//      None
 // DESCRIPTION:
-//      Default constructor.
-Scanner::Scanner(Serial &pc1, PinName _servoL, PinName _servoR, 
+//      Constructor.
+Scanner::Scanner(Serial &pc1, PinName _servoL, PinName _servoR,
     VL6180x &_shortRangeL, VL6180x &_shortRangeR, Gp2x &_longRangeL,
-    Gp2x &_longRangeR, float _period) : pc(pc1), servoL(_servoL), 
-    servoR(_servoR), shortRangeL(_shortRangeL), 
-    shortRangeR(_shortRangeR), longRangeL(_longRangeL), 
+    Gp2x &_longRangeR, float _period) : pc(pc1), servoL(_servoL),
+    servoR(_servoR), shortRangeL(_shortRangeL),
+    shortRangeR(_shortRangeR), longRangeL(_longRangeL),
     longRangeR(_longRangeR), period(_period)
 {
-    pit.attach(this, &Scanner::scan, period);
     servoL.period(1.0/50.0);
     servoR.period(1.0/50.0);
+
+    for (int i = 0; i < 13; i++)
+        DUTY[i] = 0.0300 + (i * 0.0075); // for radioshack servo
+
+    /*for (int i = 0; i < 13; i++)
+        DUTY[i] = 0.0500 + (i * DELTA_DUTY); // 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};*/
+
+    // initializing to avoid status
+    invertL = 1;
+    invertR = -1;
+    dutyL = MIN_DUTY;
+    dutyR = MAX_DUTY;
+    servoL.write(DUTY[dutyL]);
+    servoR.write(DUTY[dutyR]);
+    wait(0.2);
+    pitEnable = 1;
+    pit.attach(this, &Scanner::scan, period);
 }
 
 // FUNCTION:
 //      hunt()
 // IN-PARAMETERS:
-//      
+//      None
 // OUT-PARAMETERS:
-//      
+//      None
 // DESCRIPTION:
 //      Hunts for victim.
-int Scanner::hunt()
+void Scanner::hunt()
 {
-    return 0;
+    pitEnable = 0;
+    invertL = 1;
+    invertR = -1;
+    dutyL = HALF_DUTY;
+    dutyR = HALF_DUTY;
+    servoL.write(DUTY[dutyL]);
+    servoR.write(DUTY[dutyR]);
+    wait(0.1);
+    pitEnable = 1;
 }
 
 // FUNCTION:
 //      avoid()
 // IN-PARAMETERS:
-//      
+//      None
 // OUT-PARAMETERS:
-//      
+//      None
 // DESCRIPTION:
 //      Scans to avoid obstacles.
-int Scanner::avoid()
+void Scanner::avoid()
 {
-    return 0;
+    pitEnable = 0;
+    invertL = 1;
+    invertR = -1;
+    dutyL = MIN_DUTY;
+    dutyR = MAX_DUTY;
+    servoL.write(DUTY[dutyL]);
+    servoR.write(DUTY[dutyR]);
+    wait(0.1);
+    pitEnable = 1;
 }
 
 // FUNCTION:
 //      localize()
 // IN-PARAMETERS:
-//      
+//      None
 // OUT-PARAMETERS:
-//      
+//      None
 // DESCRIPTION:
 //      Checks localization points.
 int Scanner::localize()
 {
+    /* need to implement */
     return 0;
 }
 
 // FUNCTION:
 //      scan()
 // IN-PARAMETERS:
-//      
+//      None
 // OUT-PARAMETERS:
-//      
+//      None
 // DESCRIPTION:
-//      Checks localization points.
+//      Ticker function to make servos scan either the inward-facing
+//      90 degrees or the outward-facing 90 degrees. Distance
+//      measures are obtained from both the short range and long
+//      range sensors at 15 degree intervals.
 void Scanner::scan()
 {
-    
+    if (pitEnable == 1)
+    {
+        // obtain distance measures from sensors
+        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)) ||
+            ((dutyL == MAX_DUTY) && (dutyR == MIN_DUTY)))
+        {
+            invertL *= -1;
+            invertR *= -1;
+        }
+    }
 }
\ No newline at end of file