diff --git a/run-solleroot.py b/run-solleroot.py index 0d8658fbe249c008bcda58f73a02de36a69f5dde..31855873e4f08f6b1a6c4121f7741457ec6861e4 100644 --- a/run-solleroot.py +++ b/run-solleroot.py @@ -1,8 +1,8 @@ -#!/usr/bin/env python3 +# python3 import gatt import threading -import time +import time #not working... # BLE UUID's @@ -11,13 +11,18 @@ uart_service_uuid = '6e400001-b5a3-f393-e0a9-e50e24dcca9e' tx_characteristic_uuid = '6e400002-b5a3-f393-e0a9-e50e24dcca9e' # Write rx_characteristic_uuid = '6e400003-b5a3-f393-e0a9-e50e24dcca9e' # Notify - +# message types DATA_TYPE_COLOR_SENSOR = 4 DATA_TYPE_BUMPER = 12 DATA_TYPE_LIGHT = 13 DATA_TYPE_TOUCH = 17 DATA_TYPE_CLIFF = 20 +# color classifications +color_black = [17, 68] +color_red = [34] +color_green = [51] + class BluetoothDeviceManager(gatt.DeviceManager): robot = None # root robot device @@ -30,9 +35,12 @@ class BluetoothDeviceManager(gatt.DeviceManager): self.robot.connect() class RootDevice(gatt.Device): + # states for our robot primatives edge_following_enabled = False patterning_lines_enabled = False + # "message" allows last sensor readings to be stored in the robot object for each robot + message = "no message yet" def print_message(self): print(self.message) @@ -80,25 +88,43 @@ class RootDevice(gatt.Device): if new_data[0] == DATA_TYPE_CLIFF: type = "Cliff Sensor" # print("{}: {}".format(type, new_data)) - # Do the thing! + # Perimeter detect, then other color functions if new_data[0] == DATA_TYPE_COLOR_SENSOR: + print("new data - calling perimeter") did_adjust = self.adjust_for_perimeter_if_needed(new_data) if did_adjust: return if type == "Color Sensor" and self.edge_following_enabled: + print("new data - calling edge following") self.follow_edge(new_data) if type == "Color Sensor" and self.patterning_lines_enabled: self.pattern_dashes(new_data) - def adjust_for_perimeter_if_needed(self, data): - adjusted = False - - # TODO: Check to see if you're at perimeter! If you are, update your state and set - # adjusted = True - + 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") + n = 0 + for i in range(3,5): + if message[i] in color_black: + n += 1 + if n > 1: + self.turn_rate(-45) + adjusted = True + else: + adjusted = False + n = 0 + for i in range(16,18): + if message[i] in color_black: + n += 1 + if n > 1: + self.turn_rate(45) + adjusted = True + else: + adjusted = False 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 @@ -126,6 +152,7 @@ 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) @@ -163,9 +190,7 @@ class RootDevice(gatt.Device): #drive_root("v") turn_state = 2 else: - drive_root("f") - #manager.robot.drive_forward_slow() - #drive_root("a") + 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])