<commit_before>"""Implementations of the possible states for a robot."""

import time

from matrix import matrix


class BaseState(object):

	def __init__(self, robot):
		self.robot = robot
		self.is_oriented = False  # wut?  states aren't oriented.  robots are.

	def run(self, callback, *args, **kwargs):
		raise NotImplementedError()


class CorridorState(BaseState):

	MOVE_DURATION = 2  # seconds of movement before the next sensor measurement
	SERVO_DELAY = 0.5  # seconds of time to allow servo to complete movement


	def _sense_initial_position(self):
		"""Learn about this corridor and our place in it."""
		self.robot.driver.servo(0)
		time.sleep(SERVO_DELAY)
		right_dist = self.robot.driver.us_dist(robot.SENSOR_PIN)
		# TODO: do we need a SENSOR_DELAY?

		self.robot.driver.servo(180)
		time.sleep(SERVO_DELAY)
		left_dist = self.robot.driver.us_dist(robot.SENSOR_PIN)

		if left_dist < right_dist:
			return 'L', left_dist, left_dist + right_dist
		else:
			return 'R', right_dist, left_dist + right_dist


	def run(self, callback, *args, **kwargs):
		# Ensure the robot is stopped
		self.robot.driver.stop()

		# TODO: orient the robot, if is_oriented is False.
		# Until then, assume the robot is centered and aligned on the corridor.
		self.is_oriented = True

		# Sense the initial position:
		# Detect the distance and direction (L/R) to the nearest wall:
		closest_side, dist, width = self._sense_initial_position()

		# Initialize the robot's position/velocity state "x", expressed as:
		# [0] distance in cm from the nearest side
		# [1] lateral velocity (toward or away from the side) in cm/sec
		x = matrix([[dist], [0.]])

		# Initial uncertainty:
		# TODO: add measurement uncertainty at [0][0]
		P = matrix([[0., 0.], [0., 0.]])

		# external motion (none modeled)
		u = matrix([[0.], [0.]])

		# Next state function:
		# [0][1] = 1 -> apply full velocity to position
		F = matrix([[1., 1.], [0, 1.]])

		# Measurement function (extracts the position element from x)
		H = matrix([[1., 0.]])

		# Measurement uncertainty (none modeled)
		R = matrix([[1.]])

		# Identity matrix (matrix equivalent of 1)
		I = matrix([[1., 0.], [0., 1.]])

		while True:

			# 1. Are we in a state transition condition?
			# If so, set new state and break.

			# 2. Move
			# Start the robot, if not already moving
			self.robot.driver.fwd()

			# Pause for delay period:
			time.sleep(MOVE_DURATION)

			# Apply motion update prediction to state vector
	        x = (F * x) + u
	        P = F * P * F.transpose()

	        # 3. Sense
	        # Take a sensor measurement
	        # robot.sense()

	        # Have we reached a new state?
	        # If so, stop the robot and break
	        if False:  # detect new state here
		        robot.driver.stop()
		        # TODO: change robot's state attribute
		        # break

	        # Apply measurement update to state vector
	        # Z = matrix([measurements[n]])
	        # y = Z.transpose() - (H * x)
	        # S = H * P * H.transpose() + R
	        # K = P * H.transpose() * S.inverse()
	        # x = x + (K * y)
	        # P = (I - (K * H)) * P

	        # 4. Is an adjustment needed?
	        # If so, turn the robot
	        # robot.turn(degrees, current_speed)

			pass<commit_msg>Implemented PD-control for tracking down a corridor.<commit_after>"""Implementations of the possible states for a robot."""

import time

from matrix import matrix


class BaseState(object):

	def __init__(self, robot):
		self.robot = robot
		self.is_oriented = False  # wut?  states aren't oriented.  robots are.

	def run(self, callback, *args, **kwargs):
		raise NotImplementedError()


class CorridorState(BaseState):
	"""Actions for proceeding down a corridor.

	This algorithm uses proportional-differential control to follow the
	reference trajectory (centerline of the corridor).
	"""

	MOVE_DURATION = 2  # seconds of movement before the next sensor measurement
	SENSOR_ANGLES = {
		'L': 300,  # default angle for sensing to the left
		'R': 60    # default angle for sensing to the right
	}
	TAU_P = 0.1
	TAU_D = 0.5

	def _sense_initial_position(self):
		"""Learn about this corridor and our place in it.

		Returns a tuple (x, y), where:
			x is the total width of the corridor in cm.
			y is the displacement from the corridor center in cm.  Positive
				displacement indicates a position left of center.
		"""

		right_dist = self.robot.sense(x=90)
		left_dist = self.robot.sense(x=270)
		width = right_dist + left_dist

		return (width / 2.0 - left_dist, width)


	def run(self, callback, *args, **kwargs):
		print 'Running CorridorState'

		# Ensure the robot is stopped
		self.robot.stop()

		# TODO: orient the robot, if is_oriented is False.
		# Until then, assume the robot is centered and aligned on the corridor.
		self.is_oriented = True

		# Sense the cross-track error and width of the corridor:
		last_cte, width = self._sense_initial_position()
		print 'Corridor width: {0}'.format(width)
		if last_cte > 0:
			sensor_side = 'R'
		else:
			sensor_side = 'L'

		# Start the robot
		self.robot.fwd()

		# True when a new (non-corridor) state is detected
		new_state = False

		while not new_state:

			# Sense new cte
			angle = SENSOR_ANGLES[sensor_side]
			dist = self.robot.sense(x=angle)
			if sensor_side == 'R':
				new_cte = dist - width
			else:
				new_cte = width - dist

			# Check for new state
			# TODO: sense logic for state change detection

			# Adjust steering
			steering_factor = -TAU_P * new_cte - TAU_D * (new_cte - last_cte)
			self.robot.steer(steering_factor)
			last_cte = new_cte

			# Pause for delay period:
			time.sleep(MOVE_DURATION)

			# Print debug data
			print sensor_side, dist, last_cte, steering_factor
{'fraction_non_alphanumeric': 0.10545645330535153, 'fraction_numerical': 0.009793634137810424, 'mean_word_length': 3.0105189340813463, 'pattern_counts': {'":': 0, '<': 4, '<?xml version=': 0, '>': 5, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '449732', 'n_tokens_mistral': 2115, 'n_tokens_neox': 1837, 'n_words': 647}