def __init__(self, brick_host=None):
self.cmd = Commander(brick_finder=lambda: nxt.find_one_brick(host=brick_host))
brick = self.cmd.brick
self.platform_motor = nxt.Motor(brick, self.PLATFORM_MOTOR_PORT)
self.hand_motor = nxt.Motor(brick, self.HAND_MOTOR_PORT)
self.color_sensor_motor = nxt.Motor(brick, self.COLOR_SENSOR_MOTOR_PORT)
self.color_sensor = nxt.Color20(brick, self.COLOR_SENSOR_PORT)
self.sonar = nxt.Ultrasonic(brick, self.SONAR_PORT)
self.motors = [self.hand_motor, self.platform_motor, self.color_sensor_motor]
self.idle_all()
self.reset_rotation_counts()
class RubiksRobot(object):
COLOR_SENSOR_MOTOR_PORT = nxt.PORT_C
PLATFORM_MOTOR_PORT = nxt.PORT_A
HAND_MOTOR_PORT = nxt.PORT_B
COLOR_SENSOR_PORT = nxt.PORT_2
SONAR_PORT = nxt.PORT_1
CUBE_CHECK_PERIOD = 1
MAX_CUBE_PLACED_DISTANCE = 25
ACCURATE_TURN_STRENGTH = 40
SENSOR_TURN_STRENGTH = 20
def __init__(self, brick_host=None):
self.cmd = Commander(brick_finder=lambda: nxt.find_one_brick(host=brick_host))
brick = self.cmd.brick
self.platform_motor = nxt.Motor(brick, self.PLATFORM_MOTOR_PORT)
self.hand_motor = nxt.Motor(brick, self.HAND_MOTOR_PORT)
self.color_sensor_motor = nxt.Motor(brick, self.COLOR_SENSOR_MOTOR_PORT)
self.color_sensor = nxt.Color20(brick, self.COLOR_SENSOR_PORT)
self.sonar = nxt.Ultrasonic(brick, self.SONAR_PORT)
self.motors = [self.hand_motor, self.platform_motor, self.color_sensor_motor]
self.idle_all()
self.reset_rotation_counts()
def reset_rotation_counts(self):
for m in self.motors:
m.reset_position(relative=False)
m.reset_position(relative=True)
def idle_all(self):
for m in self.motors:
m.idle()
def is_cube_placed(self):
return self.sonar.get_distance() < self.MAX_CUBE_PLACED_DISTANCE
def wait_for_cube(self):
'''
Function returns as soon as cube is available
'''
while True:
if self.is_cube_placed():
return True
time.sleep(self.CUBE_CHECK_PERIOD)
def turn_platform(self, angle, accurate=True):
logging.info("TURN PLATFORM ISSUED: ANGLE %d" % angle)
strength = accurate and self.ACCURATE_TURN_STRENGTH or self.ACCURATE_TURN_STRENGTH*2
self.cmd.horizontal_turn_ex(angle, strength)
def turn_color_sensor(self, angle):
logging.info("TURN SENSOR ISSUED: ANGLE %d" % angle)
self.cmd.camera_turn(angle, self.SENSOR_TURN_STRENGTH)
def scan_face(self):
return scan_cube_face(self)
def get_color(self):
color = self.color_sensor.get_color()
logging.info("GETTING COLOR: %d" % color)
return color
def move_color_sensor_at_start(self):
try:
self.color_sensor_motor.turn(-64, 360, timeout=0.05, brake=True)
except nxt.BlockedException:
pass
self.color_sensor_motor.turn(5, 2, brake=True)
time.sleep(0.4)
self.color_sensor_motor.idle()
self.cmd.reset_angles()
