def main():
b = nxt.locator.find_one_brick()
mc = MotCont(b)
# mc.start()
mc.cmd(3, 50, 720)
light = Light(b, PORT_1)
light.set_illuminated(True)
# mc.stop()
while True:
try:
print "Light sensor reading: ", light.get_sample()
time.sleep(0.1)
except KeyboardInterrupt:
light.set_illuminated(False)
sys.exit(1)
def __init__(self):
self.brick = nxt.locator.find_one_brick()
self.motcont = MotCont(self.brick)
self.motcont.start()
self.power = 60
# Place the scratcher 5 cm under motor A.
self.initial_radii = cartesian_to_radii(0, 0, 5)
logger.debug("Initial radii are %s." % (self.initial_radii,))
def __init__(self, maze, bot):
super(Robot, self).__init__(180, 35, 90)
#super(Robot, self).__init__(*maze.random_free_place())
self.step_count = 0
# NXT specific
self.motcont = MotCont(bot)
self.motcont.start()
self.sonar = nxt.Ultrasonic(bot, nxt.PORT_3)
self.step = 360
self.march_power = 80
self.turn_power = 50
self.speed = 10
class Scratcher(object):
def __init__(self):
self.brick = nxt.locator.find_one_brick()
self.motcont = MotCont(self.brick)
self.motcont.start()
self.power = 60
# Place the scratcher 5 cm under motor A.
self.initial_radii = cartesian_to_radii(0, 0, 5)
logger.debug("Initial radii are %s." % (self.initial_radii,))
def move_to_radii(self, r1, r2, r3):
"Move to the given (absolute) radii."
# These are absolute radii, thanks to motcont.
radii = (r1, r2, r3)
# Sanity checks.
radii = [max(radius, 5) for radius in radii]
# Subtract the new position from the initial position. Since the software
# doesn't know we're at some point (x, y, z) to begin with, it treats the
# starting position as (0, 0, 0). The calculation here compensates for that.
radii = [(radius - initial) for radius, initial in zip(radii, self.initial_radii)]
logger.debug("Relative radii are (%s, %s, %s)." % tuple(radii))
# Convert the given radii from cm to degrees.
degrees = [radius * CM for radius in radii]
ports = (PORT_A, PORT_B, PORT_C)
for port, degrees in zip(ports, degrees):
self.motcont.move_to(port, self.power, int(degrees), smoothstart=1, brake=1)
while not all(self.motcont.is_ready(port) for port in ports):
time.sleep(0.1)
def reset(self):
"Move back to the origin."
self.move_to_radii(*self.initial_radii)
self.motcont.stop()
def move_to(self, x, y, z):
logger.debug("Moving to position (%s, %s, %s)." % (x, y, z))
y = ensure_in_triangle(x, y, z)
logger.debug("Ensured to position (%s, %s, %s)." % (x, y, z))
r1, r2, r3 = cartesian_to_radii(x, y, z)
logger.debug("Moving to radii (%s, %s, %s)." % (r1, r2, r3))
self.move_to_radii(r1, r2, r3)
def __init__(self, sock):
self.sock = sock
self.lock = Lock()
self.mc = MotCont(self)
def __init__(self):
self.b = nxt.locator.find_one_brick()
self.mc = MotCont(self.b)
class BasicMotion(object):
def __init__(self):
self.b = nxt.locator.find_one_brick()
self.mc = MotCont(self.b)
def wait_until_motors_ready(self):
ready = ( self.mc.is_ready(0) and self.mc.is_ready(1) )
while not ready:
# print "Not ready, motors still executing last command"
ready = ( self.mc.is_ready(0) and self.mc.is_ready(1) )
def main(self):
self.mc.start()
time.sleep(0.2)
# self.turn_in_place(-90)
# time.sleep(0.2)
# self.wait_until_motors_ready()
self.move_forward(20.0)
time.sleep(0.2)
self.wait_until_motors_ready()
self.turn_in_place(180)
time.sleep(0.2)
self.wait_until_motors_ready()
self.move_forward(20.0)
time.sleep(0.2)
self.wait_until_motors_ready()
self.turn_in_place(180)
time.sleep(0.2)
self.wait_until_motors_ready()
self.mc.stop()
# light = Light(self.b, PORT_1)
# light.set_illuminated(True)
# # self.turn_in_place(-90)
# # motor_a = self.mc.is_ready(0)
# # print "motor_a", motor_a
# # time.sleep(2)
# # motor_a = self.mc.is_ready(0)
# # print "motor_a", motor_a
# reflected_light = light.get_sample()
# while True:
# reflected_light = light.get_sample()
# print "reflected light value: ", reflected_light
# if reflected_light > 500:
# self.mc.stop()
# print "lost line, stopping motion"
# break
# self.stop_motion()
# self.move_backward(10.0)
# for i in range(10):
# print "Light sensor reading: ", light.get_sample()
# time.sleep(0.1)
# light.set_illuminated(False)
# except KeyboardInterrupt:
# sys.exit(1)
def __servo_rotation(self, distance):
wheel_diameter = 2.221
return (180.0 * distance) / (math.pi * wheel_diameter / 2.0)
def start_motion(self):
self.mc.start()
def stop_motion(self):
self.mc.stop()
# self.mc.set_output_state(0, 0, 0)
# self.mc.set_output_state(1, 0, 0)
def move_forward(self, distance, power = 50):
wheel_rotation = self.__servo_rotation(distance)
self.mc.cmd(3, power, int(wheel_rotation))
def move_backward(self, distance, power = 50):
wheel_rotation = self.__servo_rotation(distance)
self.mc.cmd(3, -power, int(wheel_rotation))
def turn_in_place(self, yaw, power = 50):
axle_length = 4.5
wheel_distance = (math.pi / 180.0) * abs(yaw) * (axle_length / 2.0)
wheel_rotation = self.__servo_rotation(wheel_distance)
if (yaw > 0):
self.mc.cmd(0, power, int(wheel_rotation))
self.mc.cmd(1, -power, int(wheel_rotation))
elif (yaw < 0):
self.mc.cmd(0, -power, int(wheel_rotation))
self.mc.cmd(1, power, int(wheel_rotation))
else:
print "+++ melon melon melon +++"
def __init__(self):
self.b = nxt.locator.find_one_brick()
self.mc = MotCont(self.b)
class BasicMotion(object):
def __init__(self):
self.b = nxt.locator.find_one_brick()
self.mc = MotCont(self.b)
def wait_until_motors_ready(self):
ready = (self.mc.is_ready(0) and self.mc.is_ready(1))
while not ready:
# print "Not ready, motors still executing last command"
ready = (self.mc.is_ready(0) and self.mc.is_ready(1))
def main(self):
self.mc.start()
time.sleep(0.2)
# self.turn_in_place(-90)
# time.sleep(0.2)
# self.wait_until_motors_ready()
self.move_forward(20.0)
time.sleep(0.2)
self.wait_until_motors_ready()
self.turn_in_place(180)
time.sleep(0.2)
self.wait_until_motors_ready()
self.move_forward(20.0)
time.sleep(0.2)
self.wait_until_motors_ready()
self.turn_in_place(180)
time.sleep(0.2)
self.wait_until_motors_ready()
self.mc.stop()
# light = Light(self.b, PORT_1)
# light.set_illuminated(True)
# # self.turn_in_place(-90)
# # motor_a = self.mc.is_ready(0)
# # print "motor_a", motor_a
# # time.sleep(2)
# # motor_a = self.mc.is_ready(0)
# # print "motor_a", motor_a
# reflected_light = light.get_sample()
# while True:
# reflected_light = light.get_sample()
# print "reflected light value: ", reflected_light
# if reflected_light > 500:
# self.mc.stop()
# print "lost line, stopping motion"
# break
# self.stop_motion()
# self.move_backward(10.0)
# for i in range(10):
# print "Light sensor reading: ", light.get_sample()
# time.sleep(0.1)
# light.set_illuminated(False)
# except KeyboardInterrupt:
# sys.exit(1)
def __servo_rotation(self, distance):
wheel_diameter = 2.221
return (180.0 * distance) / (math.pi * wheel_diameter / 2.0)
def start_motion(self):
self.mc.start()
def stop_motion(self):
self.mc.stop()
# self.mc.set_output_state(0, 0, 0)
# self.mc.set_output_state(1, 0, 0)
def move_forward(self, distance, power=50):
wheel_rotation = self.__servo_rotation(distance)
self.mc.cmd(3, power, int(wheel_rotation))
def move_backward(self, distance, power=50):
wheel_rotation = self.__servo_rotation(distance)
self.mc.cmd(3, -power, int(wheel_rotation))
def turn_in_place(self, yaw, power=50):
axle_length = 4.5
wheel_distance = (math.pi / 180.0) * abs(yaw) * (axle_length / 2.0)
wheel_rotation = self.__servo_rotation(wheel_distance)
if (yaw > 0):
self.mc.cmd(0, power, int(wheel_rotation))
self.mc.cmd(1, -power, int(wheel_rotation))
elif (yaw < 0):
self.mc.cmd(0, -power, int(wheel_rotation))
self.mc.cmd(1, power, int(wheel_rotation))
else:
print "+++ melon melon melon +++"
def __init__(self):
self.is_started = False
self.is_stopped = True
self.b = nxt.locator.find_one_brick()
self.mc = MotCont(self.b)
class BasicMotion(object):
def __init__(self):
self.is_started = False
self.is_stopped = True
self.b = nxt.locator.find_one_brick()
self.mc = MotCont(self.b)
def wait_until_motors_ready(self):
ready = (self.mc.is_ready(0) and self.mc.is_ready(1))
while not ready:
# print "Not ready, motors still executing last command"
ready = (self.mc.is_ready(0) and self.mc.is_ready(1))
def run(self):
self.start_motion()
self.init_light_sensor()
while True:
self.follow_line()
self.find_line()
def follow_line(self):
print "is_started: ", self.is_started
self.start_motion()
print "is_started: ", self.is_started
self.wait_until_motors_ready()
print "following line"
self.move_forward(150.0)
while True:
reflected_light = self.get_light_reading()
if reflected_light > 500:
self.stop_motion()
print "lost line, stopping motion"
return
def find_line(self):
print "is_started: ", self.is_started
self.start_motion()
print "is_started: ", self.is_started
self.wait_until_motors_ready()
print "finding line"
sweep_angle = [-10, 20, -30, 40, -60, 80, -120, 160, -240, 320]
line_found = False
is_turning = True
while not line_found:
for sweep in sweep_angle:
self.turn_in_place(sweep)
is_turning = True
while is_turning:
reflected_light = self.get_light_reading()
if reflected_light < 400:
line_found = True
self.stop_motion()
print "found line, stopping search"
return
motor_a_ready = self.mc.is_ready(0)
if motor_a_ready:
is_turning = False
def init_light_sensor(self):
self.light = Light(self.b, PORT_1)
def get_light_reading(self, illumination=True):
self.light.set_illuminated(illumination)
return self.light.get_sample()
def __servo_rotation(self, distance):
wheel_diameter = 2.221
return (180.0 * distance) / (math.pi * wheel_diameter / 2.0)
def start_motion(self):
if self.is_started == False:
self.mc.start()
self.is_started = True
time.sleep(0.5)
else:
pass
def stop_motion(self):
self.mc.stop()
self.is_started = False
time.sleep(0.2)
def move_forward(self, distance, power=50):
wheel_rotation = self.__servo_rotation(distance)
self.mc.cmd(3, power, int(wheel_rotation))
def move_backward(self, distance, power=50):
wheel_rotation = self.__servo_rotation(distance)
self.mc.cmd(3, -power, int(wheel_rotation))
def turn_in_place(self, yaw, power=50):
axle_length = 4.5
wheel_distance = (math.pi / 180.0) * abs(yaw) * (axle_length / 2.0)
wheel_rotation = self.__servo_rotation(wheel_distance)
if (yaw > 0):
self.mc.cmd(0, power, int(wheel_rotation))
self.mc.cmd(1, -power, int(wheel_rotation))
elif (yaw < 0):
self.mc.cmd(0, -power, int(wheel_rotation))
self.mc.cmd(1, power, int(wheel_rotation))
else:
print "+++ melon melon melon +++"
class Robot(Particle):
def __init__(self, maze, bot):
super(Robot, self).__init__(180, 35, 90)
#super(Robot, self).__init__(*maze.random_free_place())
self.step_count = 0
# NXT specific
self.motcont = MotCont(bot)
self.motcont.start()
self.sonar = nxt.Ultrasonic(bot, nxt.PORT_3)
self.step = 360
self.march_power = 80
self.turn_power = 50
self.speed = 10
#self.choose_random_direction()
def choose_random_direction(self):
heading = random.uniform(0, 360)
self.turn_inplace(((heading - self.h) % 360) * 6)
print("heading :" + repr(heading))
self.h = heading
def read_sensor(self, maze):
# Sonar is not exactly at the robot's baricenter.
s = self.sonar.get_sample() + ROBOT_LENGTH
print("sonar: " + repr(s - ROBOT_LENGTH))
return s
def turn_inplace(self, degrees):
self.motcont.cmd(nxt.motor.PORT_A, self.turn_power, degrees, True, True)
self.motcont.cmd(nxt.motor.PORT_C, -self.turn_power, degrees, True, True)
while not (self.motcont.is_ready(nxt.motor.PORT_A) and self.motcont.is_ready(nxt.motor.PORT_C)):
time.sleep(0.01)
def move_ahead(self, tacholimit):
self.motcont.cmd(nxt.motor.PORT_A, self.march_power, tacholimit, True, True)
self.motcont.cmd(nxt.motor.PORT_C, self.march_power, tacholimit, True, True)
while not (self.motcont.is_ready(nxt.motor.PORT_A) and self.motcont.is_ready(nxt.motor.PORT_C)):
time.sleep(0.01)
def advance_by(self, speed, checker=None):
h = self.h
r = math.radians(h)
dx = math.cos(r) * self.speed
dy = math.sin(r) * self.speed
if checker is None or checker():
self.move_ahead(self.step)
self.move_by(dx, dy)
return True
return False
def move(self, maze):
while True:
self.step_count += 1
if self.advance_by(self.speed,
checker=lambda: self.sonar.get_sample() > 20):
break
# Bumped into something or too long in same direction,
# choose random new direction
self.choose_random_direction()
