from nxt import locator
from nxt.sensor.common import PORT_1, PORT_2, PORT_3, PORT_4
from nxt.motor import PORT_A, PORT_B, PORT_C
from scripts.robot import Robot, SERVO_NICE, ON, OFF
from scripts.utils import normalize


def main():
	brick = locator.find_one_brick() # USB connection
	robot = Robot(brick)

	# Sensors
	light = robot.init_light_sensor(PORT_2)
	touch = robot.init_touch_sensor(PORT_3)

	# Motors
	servo = robot.init_servo(PORT_B)
	syn = robot.init_synchronized_motors(PORT_A, PORT_C)

	# Initial setup
	robot.set_servo(SERVO_NICE) # It's called nice because the servo movement looks freaking awesome
	robot.turn_light_sensor(ON)

	# Light sensor calibration
	black, white = robot.calibrate_light()

	# Thresholds
	lower = 4 * (10**-1)
	upper = 9 * (10**-1)
	light_level = 0

	until = lambda ls=None, ts=None: normalize(ls.get_lightness(), black, white) < upper or ts.is_pressed()
	robot.move_forward(until=until, ls=light, ts=touch)
	while robot.running:
		light_level = normalize(light.get_lightness(), black, white)
		if light_level > lower:
			robot.debug('Current light level: ', light_level)
			robot.turn_right(8, 15) # Turn right 15° to see if we correct the course
			light_level_tmp = normalize(light.get_lightness(), black, white)
			if light_level_tmp > light_level:
				robot.debug('Right turn didn\'t improve course. Turning left...')
				robot.turn_left(8, 30)

		robot.move_forward(until=until, ls=light, ts=touch)

	# WARNING: From here, the code is even more experimental.
	# I encourage you to consider this some sort of fancy pseudo-code.
	if touch.is_pressed():
		robot.debug('Found an obstacle.')
		robot.turn_right(50, 45)
		robot.move_forward(dist=10) # Dist is given in cm

if __name__ == '__main__':
	main()
Add experimental version of line.py (fancier :fire:)
Add experimental version of line.py (fancier :fire:)
