add simple tag direction and distance guesstimator

interface.py

@@ -106,7 +106,7 @@ def get_position_from_handle(self, handle):
         :param handle:
         :return:
         """
-        pos = [[],[]]
+        pos = [[], []]
         _, pos[0] = vrep.simxGetObjectPosition(self.clientID, handle, - 1, vrep.simx_opmode_streaming)
         _, pos[1] = vrep.simxGetObjectOrientation(self.clientID, handle, - 1, vrep.simx_opmode_streaming)
         return pos
@@ -121,7 +121,7 @@ def read_sensors(self):
         """
         ret = {}
         for sensor, handle in self.proximity.items():
-            _, detectionState, position, _, _  = vrep.simxReadProximitySensor(self.clientID, handle, vrep.simx_opmode_oneshot_wait)
+            _, detectionState, position, _, _  = vrep.simxReadProximitySensor(self.clientID, handle, vrep.simx_opmode_streaming)
             if not detectionState:
                 position = (0, 0, 0)
             distance = sqrt(sum([coord**2 for coord in position]))

navigator.py

@@ -0,0 +1,47 @@
+
+import cv2
+
+class Navigator():
+
+    states = ["WHERETHEFUCKAMI",]
+
+    def __init__(self, interface):
+        self.state = self.states[0]
+        self.interface = interface
+        self.find_glass = False
+
+    def iterate(self, tag_name, tag_angle, tag_distance, sensors, tag_image):
+        print(self.state, tag_name)
+        if self.state == "WHERETHEFUCKAMI":
+
+            # TODO: replace with a decent controller
+            if not (tag_name is None):
+                if tag_name == "Y.png":
+
+                    # TODO: we need to avoid abruptal direction changes on the wheels, as the
+                    # escs have a required timeout
+                    speed = 1
+                    if tag_angle is not None and tag_angle[0] > 4:
+                        self.interface.set_right_speed(- speed)
+                        self.interface.set_left_speed(speed)
+                    elif tag_angle is not None and tag_angle[0] < -4:
+                        self.interface.set_right_speed(speed)
+                        self.interface.set_left_speed(-speed)
+                    else:
+                        self.interface.set_right_speed(speed)
+                        self.interface.set_left_speed(speed)
+
+                    if sensors['fr'][0]:
+                        #got to a wall?
+                        #self.state = "GETHEFUCKINGCUP"
+                        pass
+                    print("distance:", tag_distance)
+                else:
+                    print(tag_name, tag_name is None)
+                    print("FUCK, GOT ", tag_name, tag_name is not None, type(tag_name))
+                    self.state = "GENERALIZEDCHAOS"
+                    self.interface.set_left_speed(0)
+                    self.interface.set_right_speed(0)
+                    cv2.imshow("fail", tag_image)
+        if self.state == "GETTHEFUCKINGCUP":
+            print("should be trying to get the cup, now")

neural/model/checkpoint

@@ -0,0 +1,5 @@
+model_checkpoint_path: "model.ckpt-10000"
+all_model_checkpoint_paths: "model.ckpt-1"
+all_model_checkpoint_paths: "model.ckpt-5000"
+all_model_checkpoint_paths: "model.ckpt-5001"
+all_model_checkpoint_paths: "model.ckpt-10000"