import time

from euclid import Point2
from triangula.chassis import DeadReckoning, Motion, Pose, rotate_point
from triangula.dynamics import MotionLimit
from triangula.util import IntervalCheck

"""
Task to test the basics of the patrol logic, just runs a single patrol to 30cm ahead, pauses, then moves sideways.
Hopefully. Set to not loop, so should in theory just move in an 'L' shape.
"""

[docs]    def __init__(self):

pass

"""
Create a set of simple waypoints and return the appropriate :class:triangula.tasks.patrol.PatrolTask which
will visit them in turn then exit.
"""
waypoints = [

"""
Patrol in a cool triangle pattern
"""

[docs]    def __init__(self):
self.size = 300

pass

"""
Create a set of waypoints and return an appropriate :class:triangula.tasks.patrol.PatrolTask which will visit
them in sequence.
"""
p1 = Point2(0, self.size)
p2 = rotate_point(point=p1, angle=2 * pi / 3)
p3 = rotate_point(point=p1, angle=4 * pi / 3)
colours = [30, 160, 200]
waypoints = [
stop=True),
stop=True),
]

"""
A task which manages movement through a sequence of waypoints, potentially running sub-tasks at each waypoint.
"""

ACCEL_TIME = 0.1

[docs]    def __init__(self, waypoints, loop=False, linear_offset=30, angular_offset=0.2, max_power=1.0):
"""
Create a new Patrol task, specifying a sequence of waypoints, whether to patrol continuously, and tolerances
used to determine when we've hit a waypoint and should start executing the waypoint's task.

:param waypoints:
List of :class:triangula.navigation.TaskWaypoint defining the patrol route.
:param loop:
Whether to patrol continuously, defaults to False in which case this task will return an ExitTask when it
has completed all its waypoints. If True the task will not exit, and will repeatedly run through its list
of TaskWaypoint targets until otherwise interrupted.
:param linear_offset:
Maximum linear distance away from the target Pose for each waypoint before we consider that we've hit it.
Specified in mm, defaults to 20
:param angular_offset:
Maximum angular distance away from the target Pose for each waypoint before we consider that we've hit it.
Specified in radians, defaults to 0.1
:param max_power:
A scale applied to motor speeds being sent to the chassis, defaults to 1.0 to move as fast as possible,
lower values might be helpful when testing!
"""
self.waypoints = waypoints
self.loop = loop
self.linear_offset = linear_offset
self.angular_offset = angular_offset
self.active_waypoint_index = None
self.motion_limit = None
self.pose_update_interval = IntervalCheck(interval=0.001)
self.max_power = max_power

self.active_waypoint_index = 0
self.motion_limit = MotionLimit(

# Check to see whether the minimum interval between dead reckoning updates has passed
# Do this whether we're navigating or not, as it'll also register motion performed during the execution of a
# task while at a waypoint.
if self.pose_update_interval.should_run():

waypoint = self.waypoints[self.active_waypoint_index]

# If we don't have an active sub-task, we're in waypoint seeking mode.
target_pose = waypoint.pose
# Are we close enough?
max_orientation_difference=self.angular_offset):
# Close enough, do we have to come to a complete stop first?
print 'Waypoint reached - pose is {}, target is {}'.format(self.dead_reckoning.pose, target_pose)
if waypoint.stop:
braking_start_time = time.time()
while time.time() - braking_start_time <= PatrolTask.ACCEL_TIME:
self._set_motion(motion=Motion(), context=context)
if self.pose_update_interval.should_run():
# Stop full, this should already have happened but in case it didn't we don't want the
# robot to be moving while it runs the intermediate tasks.
context.arduino.set_motor_power(0, 0, 0)
# Stopped or not, we now pick the waypoint task and start running it
else:
# Not close enough, move towards waypoint
print 'Moving towards waypoint'
scale = context.chassis.get_wheel_speeds(motion=motion).scaling
motion = Motion(translation=motion.translation * scale, rotation=motion.rotation * scale)
self._set_motion(motion=motion, context=context)
else:
# We have a sub-task, should probably run it or something. Check it's not an ExitTask first though
else:
print 'self.active_subtask is None, moving to next waypoint'
# A previous sub-task returned an ExitTask, so we're done here. Move to the next waypoint, or exit
# if we've hit all of them and we're not looping
self.active_waypoint_index += 1
if self.active_waypoint_index >= len(self.waypoints):
if self.loop:
self.active_waypoint_index = 0
else: