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()