commit 64a67b32a4193359a78ffc0f8f371661e4730402
Author: Hui Jun Tay <viki@c3po.(none)>
Date:   Tue Feb 25 12:48:24 2014 -0600

    Merged ApproachEmitter behavior with ParkScout. Still buggy.

diff --git a/scout/libscout/src/test_behaviors/ApproachEmitter.cpp b/scout/libscout/src/test_behaviors/ApproachEmitter.cpp
index 50a7442..e4f593a 100644
--- a/scout/libscout/src/test_behaviors/ApproachEmitter.cpp
+++ b/scout/libscout/src/test_behaviors/ApproachEmitter.cpp
@@ -26,6 +26,18 @@
 #include "ApproachEmitter.h"
 #define TOP_BOM 0
 #define BOT_BOM 1
+#define LEFT_EMITTER_ID 1
+#define RIGHT_EMITTER_ID 0
+
+
+#define LEFTSIDE 1
+#define RIGHTSIDE 2
+#define NEITHER 0
+
+#define CENTRED 2
+#define SEARCH 0
+#define ADJUSTED 1
+
 
 using namespace std;
 
@@ -50,7 +62,7 @@ ApproachEmitter::ApproachEmitter(string scoutname, Sensors* sensors)
 
 int ApproachEmitter::identifyPosition(
         vector<uint32_t> readings, vector<uint32_t> senders) {
-    bool identified = 0;
+    bool identified = SEARCH;
     int top_em_count = 0;
     int bot_em_count = 0;
     if (accumulate(&(readings[RIGHT.front()]),
@@ -60,13 +72,13 @@ int ApproachEmitter::identifyPosition(
             bot_em_count += (senders[i] == BOT_BOM);
         }
         if (top_em_count * bot_em_count != 0) {
-            //ROS_INFO("HAND OVER TO HUIJUN!");
+           return CENTRED;
         } else if (top_em_count > 0) {
             motors->set_sides(-50, -50, MOTOR_PERCENT);
         } else {
             motors->set_sides(50, 50, MOTOR_PERCENT);
         }
-        identified = 1;
+        identified = ADJUSTED;
     } else if (accumulate(&(readings[LEFT.front()]),
                    &(readings[LEFT.back()])+1, 0) >= 2) {
 
@@ -80,7 +92,7 @@ int ApproachEmitter::identifyPosition(
         } else {
             motors->set_sides(-50, -50, MOTOR_PERCENT);
         }
-        identified = 1;
+        identified = ADJUSTED;
     }
     return identified;
 }
@@ -90,15 +102,131 @@ void ApproachEmitter::run() {
     BomReadings bomR;
     vector<uint32_t> r;
     vector<uint32_t> s;
+    int identified = 0;
     while (ok()) {
         bomR = bom->query();
         r = bomR.readings;
         s = bomR.senders;
-        if (identifyPosition(r, s)) {
-            // do nothing
-        } else {
-            motors->set_sides(0, 50, MOTOR_PERCENT);
+	identified = identifyPosition(r, s);
+        if (identified == CENTRED) {
+	    ROS_INFO("break");
+            break;
+        } else if (identified == SEARCH) {
+            motors->set_sides(10, 40, MOTOR_PERCENT);
         }
     }
-    while (ok()) {}
+
+    update_readings();
+    start_side = get_side();
+
+    while (ok() && ((start_side == LEFTSIDE && checkBOM(3, RIGHT_EMITTER_ID)) ||
+           (start_side == RIGHTSIDE && checkBOM(6, LEFT_EMITTER_ID)))) {
+       motors->set_sides(25, 25, MOTOR_PERCENT);
+       update_readings();
+
+     }
+
+    motors->set_sides(0, 0, MOTOR_PERCENT);
+
+
+    while(ok() && reverse_park()) {
+        update_readings();
+    }
+
+
+    //motors->set_sides(0,0,MOTOR_PERCENT);
+    ROS_INFO("done");
+/*
+    while(ok()) {
+	update_readings();
+	if (at_destination()) {
+    	  motors->set_sides(0, 0, MOTOR_PERCENT);
+	  ROS_INFO("done");
+	  break;
+	}
+	else {
+	  motors->set_sides(-20,-20,MOTOR_PERCENT);
+
+	}
+    }
+*/
+}
+
+bool ApproachEmitter::reverse_park() {
+    if(checkBOM(5, LEFT_EMITTER_ID) ||
+       checkBOM(4, RIGHT_EMITTER_ID)) { //move forward
+
+        motors->set_sides(10, 10, MOTOR_PERCENT);
+    }
+    else if(checkBOM(4, LEFT_EMITTER_ID) &&
+            checkBOM(5, RIGHT_EMITTER_ID)) {
+
+        motors->set_sides(-20, -20, MOTOR_PERCENT);
+    	if (at_destination()) {
+	  return false;
+	}
+    }
+    else {
+        if (start_side == RIGHTSIDE) { //reverseright
+            motors->set_sides(-40, -10, MOTOR_PERCENT);
+        }
+        else { //reverseleft
+            motors->set_sides(-10, -40, MOTOR_PERCENT);
+
+        }
+    }
+    return true;
+}
+
+int ApproachEmitter::get_side() {
+
+    if ((readings[1] > 0 || readings[2] > 0|| readings[3] > 0) &&
+        (readings[6] == 0 && readings[7] == 0 && readings[8] == 0)) {
+        return LEFTSIDE;
+    }
+    else if ((readings[6] > 0 || readings[7] > 0|| readings[8] > 0) &&
+        (readings[1] == 0 && readings[2] == 0 && readings[3] == 0)) {
+        return RIGHTSIDE;
+    }
+    else {
+        return NEITHER;
+    }
+}
+
+bool ApproachEmitter::checkBOM(int bNum, unsigned int ID) {
+    if(readings[bNum] == true) {
+        //ROS_INFO("Emitter ID: %d", senders[bNum]);
+        return senders[bNum] == ID;
+    }
+    return false;
+
+}
+
+void ApproachEmitter::update_readings() {
+    BomReadings bomR = bom->query();
+    readings = bomR.readings;
+    senders = bomR.senders;
+
+}
+
+bool ApproachEmitter::at_destination()
+{
+    vector<uint32_t> readings = linesensor->query();
+
+/*   ROS_INFO("%d %d %d %d %d %d %d %d", readings[0], readings[1],
+	readings[2],readings[3],readings[4],readings[5],readings[6],readings[7]);
+*/
+    //changed values so it detects solution
+    if ( readings[0] > 75 ||
+         readings[1] > 75 ||
+	 readings[2] > 75 ||
+	 readings[3] > 75 ||
+         readings[4] > 75 ||
+	 readings[5] > 75 ||
+         readings[6] > 75 ||
+         readings[7] > 75 )
+    {
+        return true;
+    }
+    return false;
 }
