Commit 3a2db5ae authored by Sara Falcone's avatar Sara Falcone

fixed perimeter!

parent 6e677221
......@@ -33,13 +33,19 @@ class BluetoothDeviceManager(gatt.DeviceManager):
self.robot.connect()
class RootDevice(gatt.Device):
# states for our robot primatives
edge_following_enabled = False
patterning_lines_enabled = False
def __init__(self, mac_address, manager, managed=True):
gatt.Device.__init__(self, mac_address, manager, managed=True)
# states for our robot primatives
self.currently_adjusting_for_perimeter = False
self.edge_following_enabled = False
self.patterning_lines_enabled = False
# variables in robot object for each robot
self.message = "no message yet"
self.last_packet = []
self.dash_count = 0
# "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)
......@@ -85,14 +91,11 @@ class RootDevice(gatt.Device):
if new_data[0] == DATA_TYPE_LIGHT: type = "Light Sensor"
if new_data[0] == DATA_TYPE_TOUCH: type = "Touch Sensor"
if new_data[0] == DATA_TYPE_CLIFF: type = "Cliff Sensor"
print("{}: {}".format(type, new_data))
## print("{}: {}".format(type, new_data))
## #attempt to back out of cliff even
## if new_data[0] == DATA_TYPE_CLIFF:
## t_end = time.time() + 2
## while time.time() < t_end:
## self.drive_backwards()
## self.stop()
## self.drive_backwards()
## print("end cliff event")
# Perimeter detect, then other color functions
......@@ -100,76 +103,82 @@ class RootDevice(gatt.Device):
if new_data[0] == DATA_TYPE_COLOR_SENSOR:
# save COLOR INFO to object for future use
self.last_packet = new_data
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)
self.adjust_for_perimeter_if_needed(new_data)
## 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, self.dash_count)
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))
# THERE ARE DRIVE LEFT AND DRIVE RIGHT COMMANDS!
if all(message) in color_red:
n = 0
for i in range(3,18):
if message[i] in color_red:
n += 1
print("all - red", n)
if n > 13: #number of sensors read the color
print("in all colors are red")
t_end = time.time() + 3
while time.time() < t_end:
drive_backwards()
self.stop()
time.sleep(1)
self.drive_backwards()
self.currently_adjusting_for_perimeter = True
return
n = 0
for i in range(3,8):
if message[i] in color_red:
n += 1
if n > 3:
t_end = time.time() + 3
while time.time() < t_end:
self.steer(60,20)
self.stop()
time.sleep(1)
print("all - left", n)
if n > 3:
self.steer(90,10)
self.currently_adjusting_for_perimeter = True
print("steering left")
return
n = 0
for i in range(13,18):
if message[i] in color_red:
n += 1
if n > 3:
t_end = time.time() + 3
while time.time() < t_end:
self.steer(20,60)
self.stop()
time.sleep(1)
print("all - right", n)
if n > 3:
print("in n > 3")
self.steer(10,90)
self.currently_adjusting_for_perimeter = True
print("steering right")
return
print("OUT OF LOOP")
if self.currently_adjusting_for_perimeter:
self.currently_adjusting_for_perimeter = False
## self.stop()
self.drive_forward()
return
def pattern_dashes(self, message):
def pattern_dashes(self, message, count):
# pen down, pen up, so many times
print("IN PATTERN DASHES")
count = 0
print("count:", count)
num_dashes_wanted = 4
while count < 3: # number of lines robot will draw
if count < num_dashes_wanted:
self.pen_down()
t_end = time.time() + 5
t_end = time.time() + 2
while time.time() < t_end:
self.drive_forward()
self.stop()
time.sleep(1)
print("pen-down, driving")
self.pen_up()
t_end = time.time() + 5
t_end = time.time() + 2
while time.time() < t_end:
self.drive_forward()
self.stop()
time.sleep(1)
print("pen up, drive forward")
count += 1
print("out of loop!")
self.stop()
self.patterning_lines_enabled = False
self.stop()
else:
self.patterning_lines_enabled = False
self.dash_count += self.dash_count
def follow_edge(self, message):
......@@ -299,8 +308,10 @@ def drive_root(command):
manager.robot.follow_edge(last_packet)
if command == "pattern lines":
print ("pattern lines")
manager.robot.last_packet = 0
dash_count = manager.robot.last_packet
manager.robot.patterning_lines_enabled = True
manager.robot.pattern_dashes(last_packet)
manager.robot.pattern_dashes(last_packet, dash_count)
# start here if run as program / not imported as module
......
import time
def delay(func, time):
t_end = time.time() + time
while time.time() < t_end:
self.func()
print("OUT OF LOOP")
def func(self):
print("in LOOP")
if __name__ == "__main__":
print("in main")
length = 3
delay(func, length)
# testing passing a function into a function
# so dalay function can be used to stop drive robot commands
\ No newline at end of file
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment