示例#1
0
def mainloop_buttonMode():
	buttons = WM.state['buttons']
	# Buttons control movement and turning

	if (buttons & cwiid.BTN_HOME):
		sys.exit(0)
	if (buttons & cwiid.BTN_1):
		global MODE
		MODE = _IRLEDS_MODE

	if (car.CURRENT_DIRECTION == car.FORWARD):
		# Up button increases speed, down button decreases speed.
		# Down (past 0) sets to backward.

		if (buttons & cwiid.BTN_UP):
			car.increase_speed()
		elif (buttons & cwiid.BTN_DOWN):
			car.decrease_speed()
			if (car.CURRENT_SPEED == 0):
				car.set_direction(car.BACKWARD)
	elif (car.CURRENT_DIRECTION == car.BACKWARD):
		# Up button decreases speed, down button increases speed.
		# Up (past 0) sets to foward.

		if (buttons & cwiid.BTN_UP):
			car.decrease_speed()
			if (car.CURRENT_SPEED == 0):
				car.set_direction(car.FORWARD)
		elif (buttons & cwiid.BTN_DOWN):
			car.increase_speed()
	else:
		# Set direction based on the button pressed,
		# This will only happen on startup or after turning off the motors, so
		# the speed will start off as 0

		if (buttons & cwiid.BTN_UP):
			car.set_direction(car.FORWARD)
		elif (buttons & cwiid.BTN_DOWN):
			car.set_direction(car.BACKWARD)

	if (buttons & cwiid.BTN_LEFT):
		car.rotate_right(None)
	elif (buttons & cwiid.BTN_RIGHT):
		car.rotate_left(None)

	# Only go if the button is being held
	if (buttons & cwiid.BTN_B):
		car.enable_motors()
	else:
		car.disable_motors()

	update_leds()

	# Only poll at ~60Hz to reduce strain on batteries
	time.sleep(0.0167)
示例#2
0
def mainloop_irMode():
	buttons = WM.state['buttons']
	ir_response = WM.state['ir_src']
	global _LAST_IR_DIFF

	car.set_direction(car.FORWARD)

	if (buttons & cwiid.BTN_HOME):
		sys.exit(0)
	if (buttons & cwiid.BTN_2):
		global MODE
		MODE = _BUTTON_MODE

	# First filter out empty slots in the ir report
	ir_points = [entry['pos'] for entry in ir_response if entry is not None]

	if (len(ir_points) == 2):
		# Ideal case
		x = (ir_points[0][0] + ir_points[1][0])/2
		percent = 100.0*(x/float(cwiid.IR_X_MAX))
		car.snap_rotate(percent)

		yDiff = abs(ir_points[0][1] - ir_points[1][1])

		_LAST_IR_DIFF = yDiff
		car.snap_speed(get_speed(yDiff))
		car.enable_motors()

	elif (len(ir_points) == 1):
		# Manageable case, poor remote aim OR the robot is too close.
		# Decrease speed, keep adjusting wheel rotation. Proceed with caution.
		x = ir_points[0][0]
		percent = 100.0*(x/float(cwiid.IR_X_MAX))
		car.snap_rotate(percent)

		speed = get_speed(_LAST_IR_DIFF)/2
		car.snap_speed(speed)
		car.enable_motors()

	elif (len(ir_points) >= 2):
		# how did we end up with more than two?
		# Either there's another strong source (unlikely) or the car is sideways and we're picking
		# up both the front and back. In this case, just pick the two with the most different Y 
		# Alternately the user's aim is just really bad and they're aiming at the wrong thing.
		# Can't really plan around that.
		
		# Get the highest y value
		p1 = max(ir_points, key=lambda entry: entry[1])

		# Get the lowest y value (don't return the same point if all y values are the same)
		ir_points.remove(p1)
		p2 = min(ir_points, key=lambda entry: entry[1])

		x = (p1[0] + p2[0])/2
		percent = 100.0*(x/float(cwiid.IR_X_MAX))
		car.snap_rotate(percent)

		yDiff = abs(p1[1] - p2[1])

		_LAST_IR_DIFF = yDiff
		car.snap_speed(get_speed(yDiff))
		car.enable_motors()

	else:
		# We should stop doing anything until the user aims properly
		car.disable_motors()

	update_leds()

	time.sleep(0.0167)