def drive(velocity_high, velocity_low, radius_high, radius_low):
task1.write(Lib.DRIVE_START)
task1.wait()
task1.write(velocity_high)
task1.wait()
task1.write(velocity_low)
task1.wait()
task1.write(radius_high)
task1.wait()
task1.write(radius_low)
task1.wait()
def read_bumpers_and_wheel_drops():
bumps_wd = read_sensors(Lib.BUMP_WD)
if bumps_wd == 1:
print "Right"
elif bumps_wd == 2:
print "Left"
elif bumps_wd == 3:
print "Front"
elif bumps_wd == 4:
print "Right Wheel"
elif bumps_wd == 8:
print "Left Wheel"
task1.wait()
def write_buttons():
buttons = read_sensors(Lib.BUTTONS)
if buttons == 1:
print "Clean"
elif buttons == 2:
print "Spot"
elif buttons == 4:
print "Dock"
elif buttons == 8:
print "Minute"
elif buttons == 16:
print "Hour"
elif buttons == 32:
print "Day"
else:
print "Nothing"
task1.wait()
def main_function():
global clean_pressed, wheel_drop, left_bumper, right_bumper
global front_bumper, left_cliff, right_cliff
while 1:
if clean_pressed:
task1.wait(0.5)
clean_pressed = False
print "Starting"
while not wheel_drop and not clean_pressed and not left_cliff and not right_cliff:
if left_bumper:
lock.acquire()
task2.turn_right()
task1.turn_180_plus_rand()
lock.release()
elif right_bumper:
lock.acquire()
task2.turn_left()
task1.turn_180_plus_rand()
lock.release()
elif front_bumper:
lock.acquire()
task2.turn_random()
task1.turn_180_plus_rand()
lock.release()
lock.acquire()
task2.drive_forward()
lock.release()
task1.wait(0.125)
lock.acquire()
task2.stop_robot()
lock.release()
clean_pressed = False
wheel_drop = False
left_bumper = False
right_bumper = False
front_bumper = False
left_cliff = False
right_cliff = False
def read_sensors_2_bytes(sensor_id):
task1.write(Lib.SENSOR_START)
task1.wait()
task1.write(sensor_id)
task1.wait()
data = task1.read()
task1.wait()
data = unpack_2_bytes(data)
if data is not None:
return data
else:
return -1