diff --git a/run-solleroot.py b/run-solleroot.py index 1aad0c7859dae7b6569fd5b28a5dbbe3c105de6f..413251f57a4dcbc51b52e1824e709d9ce9461447 100644 --- a/run-solleroot.py +++ b/run-solleroot.py @@ -86,18 +86,23 @@ class RootDevice(gatt.Device): 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)) - - # save COLOR INFO to object for future use - self.last_packet = new_data - print("{}: {}".format(type, self.last_packet)) - + +## #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() +## print("end cliff event") + # 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") + # 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 did_adjust: +## return if type == "Color Sensor" and self.edge_following_enabled: print("new data - calling edge following") self.follow_edge(new_data) @@ -110,32 +115,35 @@ class RootDevice(gatt.Device): print("IN ADJUST PERIMETER") print("{}: {}".format(type, message)) - n = 0 - adjusted = False - # THERE ARE DRIVE LEFT AND DRIVE RIGHT COMMANDS! - if all(message) in color_black: - drive_backwards() - adjusted = True - return adjusted - for i in range(3,7): - if message[i] in color_black: + if all(message) in color_red: + print("in all colors are red") + t_end = time.time() + 3 + while time.time() < t_end: + drive_backwards() + self.stop() + time.sleep(1) + n = 0 + for i in range(3,8): + if message[i] in color_red: n += 1 - if n > 3: - self.turn_rate(-30) - self.drive_forward() - adjusted = True - return adjusted + if n > 3: + t_end = time.time() + 3 + while time.time() < t_end: + self.steer(60,20) + self.stop() + time.sleep(1) n = 0 - for i in range(14,18): - if message[i] in color_black: + for i in range(13,18): + if message[i] in color_red: n += 1 - if n > 3: - self.turn_rate(30) - self.drive_forward() - adjusted = True - return adjusted - return adjusted + if n > 3: + t_end = time.time() + 3 + while time.time() < t_end: + self.steer(20,60) + self.stop() + time.sleep(1) + def pattern_dashes(self, message): # pen down, pen up, so many times