diff --git a/run-solleroot.py b/run-solleroot.py index 31855873e4f08f6b1a6c4121f7741457ec6861e4..d55001ba5217d8126f34a6a66b81d465a074013d 100644 --- a/run-solleroot.py +++ b/run-solleroot.py @@ -19,9 +19,10 @@ DATA_TYPE_TOUCH = 17 DATA_TYPE_CLIFF = 20 # color classifications -color_black = [17, 68] -color_red = [34] -color_green = [51] +color_black = [17, 68, 65, 20, 49, 67, 52] +color_red = [34, 2, 32] +color_green = [51, 48, 3] +color_blue = [4, 64, 68] class BluetoothDeviceManager(gatt.DeviceManager): robot = None # root robot device @@ -41,6 +42,7 @@ class RootDevice(gatt.Device): # "message" allows last sensor readings to be stored in the robot object for each robot message = "no message yet" + last_packet = [] def print_message(self): print(self.message) @@ -88,7 +90,12 @@ class RootDevice(gatt.Device): if new_data[0] == DATA_TYPE_CLIFF: type = "Cliff Sensor" # print("{}: {}".format(type, new_data)) +## # save to object for future use +## self.last_packet = new_data +## print("{}: {}".format(type, self.last_packet)) + # Perimeter detect, then other color functions + # check if you're at the perimeter every time there is new color data if new_data[0] == DATA_TYPE_COLOR_SENSOR: print("new data - calling perimeter") did_adjust = self.adjust_for_perimeter_if_needed(new_data) @@ -101,54 +108,63 @@ class RootDevice(gatt.Device): 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 + # Check to see if you're at perimeter! + # If you are, turn and update your state print("IN ADJUST PERIMETER") + print("{}: {}".format(type, message)) + n = 0 - for i in range(3,5): + if all(message) in color_black: + drive_backwards() + adjusted = True + return adjusted + for i in range(3,7): if message[i] in color_black: n += 1 - if n > 1: - self.turn_rate(-45) + if n > 3: + self.turn_rate(-90) + self.drive_forward() adjusted = True - else: - adjusted = False + return adjusted n = 0 - for i in range(16,18): + for i in range(14,18): if message[i] in color_black: n += 1 - if n > 1: - self.turn_rate(45) + if n > 3: + self.turn_rate(90) + self.drive_forward() adjusted = True - else: - adjusted = False + return adjusted return adjusted def pattern_dashes(self, message): print("IN PATTERN DASHES") - #pen down, pen up, so many times... - count = 0 - while count < 3: # number of lines robot will draw - print(count) - t0 = time.time() #FIX TIME - t1 = time.time() - delta_t = t1 - t0 - print(delta_t) - - self.pen_down() - self.drive_forward() - while delta_t < 5000: # ms pen down - self.drive_forward() - t1 = time.time() - delta_t = t1 - t0 - t0 = time.time() - print(count) - self.pen_up() - while delta_t < 3000: # ms pen up - t1 = time.time() - delta_t = t1 - t0 - count += 1 - self.pen_up() - self.patterning_lines_enabled = False + self.drive_forward() + +## #pen down, pen up, so many times... +## count = 0 +## while count < 3: # number of lines robot will draw +## print(count) +## t0 = time.time() #FIX TIME +## t1 = time.time() +## delta_t = t1 - t0 +## print(delta_t) +## +## self.pen_down() +## self.drive_forward() +## while delta_t < 5000: # ms pen down +## self.drive_forward() +## t1 = time.time() +## delta_t = t1 - t0 +## t0 = time.time() +## print(count) +## self.pen_up() +## while delta_t < 3000: # ms pen up +## t1 = time.time() +## delta_t = t1 - t0 +## count += 1 +## self.pen_up() +## self.patterning_lines_enabled = False def follow_edge(self, message): @@ -273,6 +289,7 @@ def drive_root(command): if command == "edge": print ("edge") manager.robot.edge_following_enabled = True + # manager.robot. #send last packet to function so the robot moves if command == "pattern lines": print ("pattern lines") manager.robot.patterning_lines_enabled = True @@ -291,9 +308,8 @@ if __name__ == "__main__": print("Press letter (f,b,l,r) to drive robot (t) to turn, (s) to stop, (u or d) raise pen up or down, (q) to quit") while char != "q": char = input() # wait for keyboard input - print('predrive') + print('waiting for keyboard input') drive_root(char) - manager.robot.print_message() print('yay!') print("Quitting") manager.stop()