From 5f650672b602502d4a9d39071ad95cc3b85c4c7f Mon Sep 17 00:00:00 2001
From: Sara Falcone <sara.falcone@cba.mit.edu>
Date: Mon, 10 Dec 2018 23:59:56 -0500
Subject: [PATCH] working in edge follwing - BUGS

---
 run-solleroot.py | 92 +++++++++++++++++++++---------------------------
 time-test.py     | 22 ++++++++++--
 2 files changed, 59 insertions(+), 55 deletions(-)

diff --git a/run-solleroot.py b/run-solleroot.py
index 76d5616..b509bc7 100644
--- a/run-solleroot.py
+++ b/run-solleroot.py
@@ -1,7 +1,7 @@
 # python3
 import gatt
 import threading
-import time 
+##import time 
 
 # BLE UUID's
 root_identifier_uuid = '48c5d828-ac2a-442d-97a3-0c9822b04979'
@@ -17,10 +17,14 @@ DATA_TYPE_TOUCH = 17
 DATA_TYPE_CLIFF = 20
 
 # color classifications
-color_black = [17, 68, 65, 20, 49, 67, 52]
-color_red = [34, 2, 32]
+color_black = [17, 68, 65, 20, 49, 67, 52, 16, 1]
+color_red = [34, 2, 32, 33, 18]
 color_green = [51, 48, 3]
 color_blue = [4, 64, 68]
+               
+# uncategorized
+# color_white = [0,
+# 33, 18
 
 class BluetoothDeviceManager(gatt.DeviceManager):
     robot = None # root robot device
@@ -110,17 +114,12 @@ class RootDevice(gatt.Device):
         if type == "Color Sensor" and self.edge_following_enabled:
             print("new data - calling edge following")
             self.follow_edge(new_data)
-##        elif type == "Color Sensor" and self.patterning_lines_enabled:
-##            print("new data - calling patterning")
-##            self.dash_count(self.dash_count)
-##            self.pattern_dashes(new_data)
-        
       
     def adjust_for_perimeter_if_needed(self, message):
         # Check to see if you're at perimeter!
         # If you are, turn and update your state
-        print("IN ADJUST PERIMETER")
-        print("{}: {}".format(type, message))
+##        print("IN ADJUST PERIMETER")
+##        print("{}: {}".format(type, message))
         
         n = 0
         for i in range(3,18):
@@ -131,7 +130,7 @@ class RootDevice(gatt.Device):
             self.drive_forward()
             return
         if not self.currently_adjusting_for_perimeter:
-            self.pen_up()
+##            self.pen_up()
             self.currently_adjusting_for_perimeter = True
             
         if n > 13: #number of sensors read the color
@@ -156,12 +155,9 @@ class RootDevice(gatt.Device):
             return
             
     def do_dash_movement(self):
-        print('doin it')
         threading.Timer(1.0, self.do_dash_movement).start()
        # if self.total_dash_count >= MAX_TOTAL_DASHES:
            # return
-        if self.currently_adjusting_for_perimeter:
-            return
         if(self.dash_count % 2 == 0):
             print("pen-down, driving")
             self.pen_down()
@@ -174,44 +170,36 @@ class RootDevice(gatt.Device):
 
     def follow_edge(self, message):
         print("IN FOLLOW EDGE")
-        if message[3] != 0 or message[4] != 0:
-            #drive_root("l")
-            self.turn_rate(-45)
-            #manager.robot.drive_left_slow()
-            #drive_root("z")
-            turn_state = 1  
-        elif message[5] != 0 or message[6] != 0:
-            #drive_root("l")
-            self.turn_rate(-30)
-            #manager.robot.drive_left_slow_slow()
-            #drive_root("x")
-            turn_state = 1                
-            #drive_root("f")
-            #print("test1")
-        elif message[7] != 0 or message[8] != 0:
-            #drive_root("f") # calling the member function versus out into global space
-            self.drive_forward()
-        elif message[9] != 0 or message[10] != 0:
-            self.turn_rate(30)
+        
+        n = 0
+        for i in range(9,12)
+            if message[i] in color_black:
+                n += 1
+        if n > 1
+            self.drive_forward:
+            return
             
-        elif message[12] != 0 or message[11] != 0:
-            self.turn_rate(45)
-        elif message[14] != 0 or message[13] != 0:
-            #drive_root("r")
-            self.turn_rate(60)
-            #manager.robot.drive_right_slow_slow()
-            #drive_root("c")
-            turn_state = 2                
-            #drive_root("f")
-            #print("test2")
-        elif message[16] != 0 or message[15] != 0:
-            #drive_root("r")
-            self.turn_rate(60)
-            #manager.robot.drive_right_slow()
-            #drive_root("v")
-            turn_state = 2
-        else:
-             self.drive_forward()
+        left_n = 0
+        for i in range(3,8):
+            if message[i] in color_black:
+                left_n += 1
+        right_n = 0
+        for i in range(13,18):
+            if message[i] in color_black:
+                right_n += 1
+        if left_n > 21 and right_n > 1:
+            self.drive_forward()
+            return
+        if left_n > 1:
+            self.steer(70,10)
+            print("steering left")
+            return    
+        if right_n > 1:
+            self.steer(10,70)
+            print("steering right")
+            return
+        self.drive_forward()
+
 
     def drive_forward(self):
         self.tx_characteristic.write_value([0x01, 0x04, 0x00, 0x00, 0x00, 0x00, 0x64, 0x00, 0x00, 0x00, 0x64, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xD1])
@@ -296,7 +284,7 @@ def drive_root(command):
     if command == "edge":
         print ("edge")
         manager.robot.edge_following_enabled = True
-        manager.robot.follow_edge(manager.robot.last_packet)
+        manager.robot.drive_forward()
     if command == "pattern lines":
         print ("pattern lines")
         manager.robot.drive_forward()
diff --git a/time-test.py b/time-test.py
index 6659ea6..8c179a7 100644
--- a/time-test.py
+++ b/time-test.py
@@ -1,5 +1,21 @@
 import time
+import threading
 
+sara_is_pedantic = 0
+
+def printit():
+  threading.Timer(2.0, printit).start()
+  print("Hello, World! {}".format(sara_is_pedantic))
+  global sara_is_pedantic
+  sara_is_pedantic +=1
+  if sara_is_pedantic == 4:
+      print("OUR WORK HERE SHOULD BE DONE?")
+      return
+
+def printit_2():
+  threading.Timer(5.0, printit_2).start()
+  print("Hello, World! 5")
+  
 def delay(func, time):
     t_end = time.time() + time
     while time.time() < t_end:
@@ -13,9 +29,9 @@ def func(self):
 if __name__ == "__main__":
     print("in main")
     length = 3
-    delay(func, length)
+##    delay(func, length)
+    printit()
+##    printit_2()
     
     
-    # testing passing a function into a function
-    # so dalay function can be used to stop drive robot commands
     
\ No newline at end of file
-- 
GitLab