commit fb61cb17b59a045fa11e5f7c9402f322af2183ea
Author: Yuyang (Misty) Guo <yuyangg@andrew.cmu.edu>
Date:   Fri Feb 21 23:09:41 2014 -0500

    Add first stage of charging behavoir

diff --git a/scout/libscout/src/test_behaviors/ApproachEmitter.cpp b/scout/libscout/src/test_behaviors/ApproachEmitter.cpp
index 4f2db02..50a7442 100644
--- a/scout/libscout/src/test_behaviors/ApproachEmitter.cpp
+++ b/scout/libscout/src/test_behaviors/ApproachEmitter.cpp
@@ -24,39 +24,81 @@
  */
 
 #include "ApproachEmitter.h"
-#define BOM_W1 5
-#define BOM_W2 10
-#define BOM_W3 15
-#define BOM_W4 20
-#define BOM_W5 30
+#define TOP_BOM 0
+#define BOT_BOM 1
 
 using namespace std;
 
-ApproachEmitter::ApproachEmitter(string scoutname, Sensors* sensors) 
+ApproachEmitter::ApproachEmitter(string scoutname, Sensors* sensors)
     : Behavior(scoutname, "Approach Emitter", sensors) {
     name = scoutname;
     scout_pos = new pos;
+
+    // initallize the side index values :D UGLY!!! But best solution so far
+    FRONT.push_back(0);
+    LEFT.push_back(1);
+    LEFT.push_back(2);
+    LEFT.push_back(3);
+    BACK.push_back(4);
+    BACK.push_back(5);
+    RIGHT.push_back(6);
+    RIGHT.push_back(7);
+    RIGHT.push_back(8);
+    FRONT.push_back(9);
+
+}
+
+int ApproachEmitter::identifyPosition(
+        vector<uint32_t> readings, vector<uint32_t> senders) {
+    bool identified = 0;
+    int top_em_count = 0;
+    int bot_em_count = 0;
+    if (accumulate(&(readings[RIGHT.front()]),
+                   &(readings[RIGHT.back()])+1, 0) >= 2) {
+        for (int i=RIGHT.front(); i<RIGHT.back(); i++) {
+            top_em_count += (senders[i] == TOP_BOM);
+            bot_em_count += (senders[i] == BOT_BOM);
+        }
+        if (top_em_count * bot_em_count != 0) {
+            //ROS_INFO("HAND OVER TO HUIJUN!");
+        } else if (top_em_count > 0) {
+            motors->set_sides(-50, -50, MOTOR_PERCENT);
+        } else {
+            motors->set_sides(50, 50, MOTOR_PERCENT);
+        }
+        identified = 1;
+    } else if (accumulate(&(readings[LEFT.front()]),
+                   &(readings[LEFT.back()])+1, 0) >= 2) {
+
+        for (int i=LEFT.front(); i<LEFT.back(); i++) {
+            top_em_count += (senders[i] == TOP_BOM);
+            bot_em_count += (senders[i] == BOT_BOM);
+        }
+        if (top_em_count * bot_em_count != 0) {
+        } else if (top_em_count > 0) {
+            motors->set_sides(50, 50, MOTOR_PERCENT);
+        } else {
+            motors->set_sides(-50, -50, MOTOR_PERCENT);
+        }
+        identified = 1;
+    }
+    return identified;
 }
 
 void ApproachEmitter::run() {
     ROS_INFO("BOM Main Running");
     BomReadings bomR;
-    int r_weighted, l_weighted;
-    int speed = 20;
     vector<uint32_t> r;
     vector<uint32_t> s;
     while (ok()) {
         bomR = bom->query();
         r = bomR.readings;
         s = bomR.senders;
-        ROS_INFO("BOM Main Running");
-        if (accumulate(r.begin(), r.end(), 0)) {
-            for (int i=0; i<10; ++i) {
-                ROS_INFO("BOM %d reading is %d sender is %d", i, r[i], s[i]);
-            }
-           
-           //motors->set_sides(min(r_weighted+speed, 100),
-           //                  min(l_weighted+speed, 102),  MOTOR_PERCENT);
+        if (identifyPosition(r, s)) {
+            // do nothing
+        } else {
+            motors->set_sides(0, 50, MOTOR_PERCENT);
         }
     }
+    while (ok()) {}
 }
diff --git a/scout/libscout/src/test_behaviors/ApproachEmitter.h b/scout/libscout/src/test_behaviors/ApproachEmitter.h
index 4137065..f663429 100644
--- a/scout/libscout/src/test_behaviors/ApproachEmitter.h
+++ b/scout/libscout/src/test_behaviors/ApproachEmitter.h
@@ -42,10 +42,16 @@ class ApproachEmitter: public Behavior
 {
     public:
     ApproachEmitter(std::string scoutname, Sensors* sensors);
-    void run(); 
-    private:    
+    void run();
+    private:
+    int identifyPosition(std::vector<uint32_t> readings,
+                     std::vector<uint32_t> senders);
     std::string name;
     pos* scout_pos;
+    std::vector<uint8_t> LEFT;
+    std::vector<uint8_t> RIGHT;
+    std::vector<uint8_t> FRONT;
+    std::vector<uint8_t> BACK;
 
 };
 #endif
