Revision 3:992558a021d7, committed 2016-03-21

Comitter:

j_j205

Date:

Mon Mar 21 01:32:09 2016 +0000

Parent:

2:b281034eda86

Child:

4:6a8d80954323

Commit message:

3/20/16 updated code jj

Changed in this revision

--- a/scanner.cpp	Tue Feb 23 23:35:41 2016 +0000
+++ b/scanner.cpp	Mon Mar 21 01:32:09 2016 +0000
@@ -23,40 +23,50 @@
     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
+    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};
 
-    /*for (int i = 0; i < 13; i++)
-        DUTY[i] = 0.0500 + (i * DELTA_DUTY); // 0.05 = min duty cycle
+    for (int i = 0; i < 13; 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};*/
+        // 0.1000};
+
+    obstacle = 0;
+    distLeft = 0.0;
+    distRight = 0.0;
+    distForward = 0.0;
 
     // initializing to avoid status
-    invertL = 1;
-    invertR = -1;
-    dutyL = MIN_DUTY;
-    dutyR = MAX_DUTY;
+    avoidFlag = 1;
+    huntFlag = 0;
+    invertL = -1;
+    invertR = 1;
+    dutyL = MAX_DUTY;
+    dutyR = MIN_DUTY;
     servoL.write(DUTY[dutyL]);
     servoR.write(DUTY[dutyR]);
     wait(0.2);
     pitEnable = 1;
-    pit.attach(this, &Scanner::scan, period);
+    scanPit.attach(this, &Scanner::scan, period);
 }
 
 // FUNCTION:
-//      hunt()
+//      huntMode()
 // IN-PARAMETERS:
 //      None
-// OUT-PARAMETERS:
 //      None
 // DESCRIPTION:
 //      Hunts for victim.
-void Scanner::hunt()
+void Scanner::huntMode()
 {
     pitEnable = 0;
-    invertL = 1;
-    invertR = -1;
+    avoidFlag = 0;
+    huntFlag = 1;
+    invertL = -1;
+    invertR = 1;
     dutyL = HALF_DUTY;
     dutyR = HALF_DUTY;
     servoL.write(DUTY[dutyL]);
@@ -66,20 +76,22 @@
 }
 
 // FUNCTION:
-//      avoid()
+//      avoidMode()
 // IN-PARAMETERS:
 //      None
 // OUT-PARAMETERS:
 //      None
 // DESCRIPTION:
 //      Scans to avoid obstacles.
-void Scanner::avoid()
+void Scanner::avoidMode()
 {
     pitEnable = 0;
-    invertL = 1;
-    invertR = -1;
-    dutyL = MIN_DUTY;
-    dutyR = MAX_DUTY;
+    avoidFlag = 1;
+    huntFlag = 0;
+    invertL = -1;
+    invertR = 1;
+    dutyL = MAX_DUTY;
+    dutyR = MIN_DUTY;
     servoL.write(DUTY[dutyL]);
     servoR.write(DUTY[dutyR]);
     wait(0.1);
@@ -94,10 +106,23 @@
 //      None
 // DESCRIPTION:
 //      Checks localization points.
-int Scanner::localize()
+void Scanner::localize()
 {
-    /* need to implement */
-    return 0;
+    pitEnable = 0;
+    dutyL = MIN_DUTY;
+    dutyR = MAX_DUTY;
+    servoL.write(DUTY[dutyL]);
+    servoR.write(DUTY[dutyR]);
+    wait(0.1);
+    /* distLeft = longRangL.read();
+       distRight = longRangeR.read(); */
+    dutyL = HALF_DUTY;
+    dutyR = HALF_DUTY;
+    servoL.write(DUTY[dutyL]);
+    servoR.write(DUTY[dutyR]);
+    wait(0.1);
+    /* distForward = ( longRangeL.read() + longRangeR.read() )/2.0 */
+    pitEnable = 1;
 }
 
 // FUNCTION:
@@ -116,11 +141,16 @@
     if (pitEnable == 1)
     {
         // obtain distance measures from sensors
+        // check avoid flag
+        // add obstacle avoid code
+        // check hunt flag
+        // 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)) ||
             ((dutyL == MAX_DUTY) && (dutyR == MIN_DUTY)))
@@ -129,4 +159,4 @@
             invertR *= -1;
         }
     }
-}
\ No newline at end of file
+}

--- a/scanner.h	Tue Feb 23 23:35:41 2016 +0000
+++ b/scanner.h	Mon Mar 21 01:32:09 2016 +0000
@@ -10,9 +10,12 @@
     Scanner(Serial &pc1, PinName _servoL, PinName _servoR,
     VL6180x &_shortRangeL, VL6180x &_shortRangeR, Gp2x &_longRangeL,
     Gp2x &_longRangeR, float _period = 0.2);
-    void hunt();
-    void avoid();
-    int localize();
+    void huntMode();
+    void avoidMode();
+    void localize();
+    float getDistLeft() {return distLeft;}
+    float getDistRight() {return distRight;}
+    float getDistForward() {return distForward;}
 
 private:
     static const int MIN_DUTY = 0;
@@ -27,6 +30,9 @@
     int invertR;
     int dutyL;
     int dutyR;
+    float distLeft;
+    float distRight;
+    float distForward;
     Serial &pc;
     PwmOut servoL;
     PwmOut servoR;
@@ -35,11 +41,14 @@
     Gp2x &longRangeL;
     Gp2x &longRangeR;
     float period;
+    bool obstacle;
+    bool huntFlag;
+    bool avoidFlag;
 
-    Ticker pit; // periodic interrupt timer
+    Ticker scanPit; // periodic interrupt timer
 
     void scan();
 
 };
 
-#endif // SCANNER_H
\ No newline at end of file
+#endif // SCANNER_H