From 3b0b5106df17a1101007ca395ee9eb2cf53b701a Mon Sep 17 00:00:00 2001
From: Ahmed Samara <asamara@ncsu.edu>
Date: Sat, 26 Mar 2016 23:41:00 -0400
Subject: [PATCH 1/2] instantiated camera in nav.py

---
 bot/config.yaml                               |  4 +--
 bot/hardware/complex_hardware/SeventhDOF.py   |  7 +++++
 .../complex_hardware/camera_reader.py         | 31 +++++++++++--------
 bot/hardware/complex_hardware/robot_arm.py    | 21 ++++++++++---
 bot/navigation/nav.py                         |  6 ++++
 5 files changed, 49 insertions(+), 20 deletions(-)

diff --git a/bot/config.yaml b/bot/config.yaml
index 650527f..48dc9b8 100644
--- a/bot/config.yaml
+++ b/bot/config.yaml
@@ -49,8 +49,8 @@ omni_drive_motors: {
     south: { board_num: 2, motor_num: 2 , invert: True},
     east: { board_num: 0, motor_num: 2 , invert: True},
     west: { board_num: 0, motor_num: 1 },
-    north: { board_num: 2, motor_num: 1 }
-
+    north: { board_num: 2, motor_num: 1 },
+    camera: generic_cam
 }
 
 IR : {
diff --git a/bot/hardware/complex_hardware/SeventhDOF.py b/bot/hardware/complex_hardware/SeventhDOF.py
index 1606ead..c7eee8c 100644
--- a/bot/hardware/complex_hardware/SeventhDOF.py
+++ b/bot/hardware/complex_hardware/SeventhDOF.py
@@ -16,6 +16,13 @@ def __init__(self):
         
         self.rail_DMCC = pyDMCC.DMCC(1)    
         self.rail_motor = self.rail_DMCC.motors[motor_num]
+
+    @lib.api_call
+    def get_position(self):
+        return self.rail_motor.position
+
+
+
     @lib.api_call 
     def Orientor(self,Position):
         
diff --git a/bot/hardware/complex_hardware/camera_reader.py b/bot/hardware/complex_hardware/camera_reader.py
index d8af1b1..449255c 100644
--- a/bot/hardware/complex_hardware/camera_reader.py
+++ b/bot/hardware/complex_hardware/camera_reader.py
@@ -16,17 +16,7 @@
 from bot.hardware.complex_hardware.QRCode2 import Block
 from bot.hardware.complex_hardware.partial_qr import *
 
-def find_name(symlink):
-    # find where symlink is pointing (/dev/vide0, video1, etc)
-    cmd = "readlink -f /dev/" + symlink 
-    process = subprocess.Popen(cmd.split(), stdout=subprocess.PIPE)
-    out = process.communicate()[0]
-
-    #extract ints from name video0, etc
-    nums = [int(x) for x in out if x.isdigit()]
-    # There should nto be more than one digit
-    interface_num = nums[0]    
-    return interface_num
+
 
 class Camera(object):
 
@@ -42,7 +32,7 @@ def __init__(self, cam_config):
         udev_name = cam_config["udev_name"]
         print udev_name
 
-        cam_num = find_name(udev_name)
+        cam_num = self.find_cam_num(udev_name)
         
         # extract calib data from cam_config
         self.a = cam_config["a"]
@@ -59,7 +49,22 @@ def __init__(self, cam_config):
         self.scanner = zbar.ImageScanner()
         self.scanner.parse_config('enable')
         
-        
+    def find_name(self, symlink):
+            # find where symlink is pointing (/dev/vide0, video1, etc)
+        cmd = "readlink -f /dev/" + symlink 
+        process = subprocess.Popen(cmd.split(), 
+                                   stdout=subprocess.PIPE)
+        out = process.communicate()[0]
+
+        #extract ints from name video0, etc
+        nums = [int(x) for x in out if x.isdigit()]
+        # There should nto be more than one digit
+        if len(nums) != 0:
+            print "error with camera interfaces"
+            return
+        interface_num = nums[0]    
+        return interface_num        
+
     def apply_filters(self, frame):
         """Attempts to improve viewing by applying filters """
         # grayscale
diff --git a/bot/hardware/complex_hardware/robot_arm.py b/bot/hardware/complex_hardware/robot_arm.py
index ba23dba..518cf65 100644
--- a/bot/hardware/complex_hardware/robot_arm.py
+++ b/bot/hardware/complex_hardware/robot_arm.py
@@ -63,6 +63,11 @@ def __init__(self, arm_config):
 
         self.hopper = [None, None, None, None]
 
+    @lib.api_call
+    def get_props(self):
+        print "rail pos", self.rail.get_position()
+        
+
     @property
     def joints(self):
         return self.__joints
@@ -74,6 +79,8 @@ def joints(self, vals):
         if len(vals) == 5:
             self.__joints = vals
         else:
+            vals[:] = [0 for i in a if i < 0]
+            vals[:] = [180 for i in a if i > 180]
             self.__joints[:len(vals)] = vals
         print "Joints to be sent: ", vals
         self.servo_cape.transmit_block([0] + self.__joints)
@@ -128,8 +135,8 @@ def joint_center_on_qr(self):
         
         # Correction constants for P(ID) controller.
         # unlikely that we'll bother using I or D
-        p_x = 10
-        p_y = 10
+        p_x = 50
+        p_y = 6
 
         while True:
             ret = self.cam.QRSweep()
@@ -141,7 +148,7 @@ def joint_center_on_qr(self):
                 dy = ret.tvec[1]
                 
                 if abs(dx) > 0.1:
-                    self.joints[0] += p_x * dx
+                    self.move_rail(p_x * dx)
                 if abs(dy) > 0.1:
                     self.joints[3] += p_y * dy
                 #print "Joints = ", self.joints
@@ -551,7 +558,11 @@ def check_hopper(self):
     @lib.api_call
     def orient(self, pos):
         self.rail.Orientor(pos)
-    
+
+    @lib.api_call
+    def move_rail(self, displacement):
+        return self.rail.DisplacementMover(displacement)
+ 
     @lib.api_call
     def GrabQR(self):
         self.cam.QRSweep()
@@ -588,4 +599,4 @@ def color_test_loop(self, hopper_pos):
             else: 
                 print "Error: No color Found."
 
-    
\ No newline at end of file
+    
diff --git a/bot/navigation/nav.py b/bot/navigation/nav.py
index dfd8c84..bac4957 100644
--- a/bot/navigation/nav.py
+++ b/bot/navigation/nav.py
@@ -8,6 +8,8 @@
 import os.path
 import yaml
 
+from bot.hardware.complex_hardware import Camera
+
 bound = lambda x, l, u: l if x < l else u if x > u else x
 
 MAX_VALUE = 800
@@ -34,6 +36,10 @@ def __init__(self, rail_cars=0):
         mapping = ["EXIT", "west", "east", "EXIT"]
         self.rail_cars_side = mapping[rail_cars]
 
+        # camera for checking
+        c_config = self.config["generic_cam"]
+        self.cam = Camera(c_config)
+
     def stop_unused_motors(self, direction):
         direction = direction.lower()
         if direction == "north" or direction == "south":

From 1585b59433a6da12ef0d4012788edd92e5953cff Mon Sep 17 00:00:00 2001
From: Ahmed Samara <asamara@ncsu.edu>
Date: Mon, 28 Mar 2016 12:53:01 -0400
Subject: [PATCH 2/2] added bbox check

---
 bot/navigation/nav.py | 24 +++++++++++++++++++++++-
 1 file changed, 23 insertions(+), 1 deletion(-)

diff --git a/bot/navigation/nav.py b/bot/navigation/nav.py
index bac4957..a53e7ca 100644
--- a/bot/navigation/nav.py
+++ b/bot/navigation/nav.py
@@ -36,10 +36,32 @@ def __init__(self, rail_cars=0):
         mapping = ["EXIT", "west", "east", "EXIT"]
         self.rail_cars_side = mapping[rail_cars]
 
-        # camera for checking
+        # camera for checking block
         c_config = self.config["generic_cam"]
         self.cam = Camera(c_config)
 
+    @lib.api_call
+    def check_current_car(self, expected_color):
+        """ Verifies that car is currenly facing a bin"""
+        frame = self.cam.get_current_frame()
+        qr_list = self.cam.get_qr_list(frame)
+        
+        # Should not happen unless far away
+        if len(qr_list) > 2:
+            return False
+        
+        # assure that at least 2 are same
+        colors = {"red":0, "blue": 0, "green":0, "yellow":0}
+        for q in qr_list:
+            colors[q.value] += 1
+
+        if any( i >= 2 for i in colors.itervalues()):
+            return True
+
+        # If none of these conditions met, assum not lined up
+            return False        
+
+
     def stop_unused_motors(self, direction):
         direction = direction.lower()
         if direction == "north" or direction == "south":