def init_task(self, context):
self.active_waypoint_index = 0
self.dead_reckoning = DeadReckoning(chassis=context.chassis,
counts_per_revolution=3310)
self.motion_limit = MotionLimit(
linear_acceleration_limit=context.chassis.
get_max_translation_speed() / PatrolTask.ACCEL_TIME,
angular_acceleration_limit=context.chassis.get_max_rotation_speed(
) / PatrolTask.ACCEL_TIME)
def init_task(self, context):
# Maximum translation speed in mm/s
self.max_trn = context.chassis.get_max_translation_speed()
# Maximum rotation speed in radians/2
self.max_rot = context.chassis.get_max_rotation_speed()
self._set_relative_motion(context)
self.dead_reckoning = DeadReckoning(chassis=context.chassis, counts_per_revolution=3310)
self.motion_limit = MotionLimit(
linear_acceleration_limit=context.chassis.get_max_translation_speed() / ManualMotionTask.ACCEL_TIME,
angular_acceleration_limit=context.chassis.get_max_rotation_speed() / ManualMotionTask.ACCEL_TIME)
self.rate_limit = RateLimit(limit_function=RateLimit.fixed_rate_limit_function(1 / ManualMotionTask.ACCEL_TIME))
self.limit_mode = 0
class PatrolTask(Task):
"""
A task which manages movement through a sequence of waypoints, potentially running sub-tasks at each waypoint.
"""
ACCEL_TIME = 0.1
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!
"""
super(PatrolTask, self).__init__(task_name='Patrol', requires_compass=False)
self.waypoints = waypoints
self.loop = loop
self.linear_offset = linear_offset
self.angular_offset = angular_offset
self.active_waypoint_index = None
self.active_subtask = None
self.dead_reckoning = None
self.motion_limit = None
self.pose_update_interval = IntervalCheck(interval=0.001)
self.subtask_tick = 0
self.max_power = max_power
def init_task(self, context):
self.active_waypoint_index = 0
self.dead_reckoning = DeadReckoning(chassis=context.chassis, counts_per_revolution=3310)
self.motion_limit = MotionLimit(
linear_acceleration_limit=context.chassis.get_max_translation_speed() / PatrolTask.ACCEL_TIME,
angular_acceleration_limit=context.chassis.get_max_rotation_speed() / PatrolTask.ACCEL_TIME)
def poll_task(self, context, tick):
# 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():
self.dead_reckoning.update_from_counts(context.arduino.get_encoder_values())
# print self.dead_reckoning.pose
waypoint = self.waypoints[self.active_waypoint_index]
# If we don't have an active sub-task, we're in waypoint seeking mode.
if self.active_subtask is None:
target_pose = waypoint.pose
# Are we close enough?
if self.dead_reckoning.pose.is_close_to(target_pose, max_distance=self.linear_offset,
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():
self.dead_reckoning.update_from_counts(context.arduino.get_encoder_values())
# 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
self.active_subtask = waypoint.task
print 'Task is {}'.format(self.active_subtask)
if self.active_subtask is None:
self.active_subtask = ExitTask()
self.active_subtask.init_task(context=context)
self.subtask_tick = 0
else:
# Not close enough, move towards waypoint
print 'Moving towards waypoint'
motion = self.dead_reckoning.pose.pose_to_pose_motion(to_pose=target_pose, time_seconds=0.01)
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
if isinstance(self.active_subtask, ExitTask):
print 'self.active_subtask is an ExitTask, setting to None'
self.active_subtask = None
else:
print 'Polling subtask, tick {}'.format(self.subtask_tick)
task_result = self.active_subtask.poll_task(context=context, tick=self.subtask_tick)
self.subtask_tick += 1
if task_result is not None:
self.subtask_tick = 0
self.active_subtask = task_result
self.active_subtask.init_task(context=context)
if self.active_subtask is None:
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:
return ExitTask()
def _set_motion(self, motion, context):
"""
Using the motion limit traction control, apply the specified motion to the chassis.
"""
motion = self.motion_limit.limit_and_return(motion=motion)
wheel_speeds = context.chassis.get_wheel_speeds(motion=motion)
speeds = wheel_speeds.speeds
power = [speeds[i] / context.chassis.wheels[i].max_speed for i in range(0, 3)]
context.arduino.set_motor_power(power[0] * self.max_power,
power[1] * self.max_power,
power[2] * self.max_power)
def init_task(self, context):
self.active_waypoint_index = 0
self.dead_reckoning = DeadReckoning(chassis=context.chassis, counts_per_revolution=3310)
self.motion_limit = MotionLimit(
linear_acceleration_limit=context.chassis.get_max_translation_speed() / PatrolTask.ACCEL_TIME,
angular_acceleration_limit=context.chassis.get_max_rotation_speed() / PatrolTask.ACCEL_TIME)
class PatrolTask(Task):
"""
A task which manages movement through a sequence of waypoints, potentially running sub-tasks at each waypoint.
"""
ACCEL_TIME = 0.1
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!
"""
super(PatrolTask, self).__init__(task_name='Patrol',
requires_compass=False)
self.waypoints = waypoints
self.loop = loop
self.linear_offset = linear_offset
self.angular_offset = angular_offset
self.active_waypoint_index = None
self.active_subtask = None
self.dead_reckoning = None
self.motion_limit = None
self.pose_update_interval = IntervalCheck(interval=0.001)
self.subtask_tick = 0
self.max_power = max_power
def init_task(self, context):
self.active_waypoint_index = 0
self.dead_reckoning = DeadReckoning(chassis=context.chassis,
counts_per_revolution=3310)
self.motion_limit = MotionLimit(
linear_acceleration_limit=context.chassis.
get_max_translation_speed() / PatrolTask.ACCEL_TIME,
angular_acceleration_limit=context.chassis.get_max_rotation_speed(
) / PatrolTask.ACCEL_TIME)
def poll_task(self, context, tick):
# 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():
self.dead_reckoning.update_from_counts(
context.arduino.get_encoder_values())
# print self.dead_reckoning.pose
waypoint = self.waypoints[self.active_waypoint_index]
# If we don't have an active sub-task, we're in waypoint seeking mode.
if self.active_subtask is None:
target_pose = waypoint.pose
# Are we close enough?
if self.dead_reckoning.pose.is_close_to(
target_pose,
max_distance=self.linear_offset,
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():
self.dead_reckoning.update_from_counts(
context.arduino.get_encoder_values())
# 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
self.active_subtask = waypoint.task
print 'Task is {}'.format(self.active_subtask)
if self.active_subtask is None:
self.active_subtask = ExitTask()
self.active_subtask.init_task(context=context)
self.subtask_tick = 0
else:
# Not close enough, move towards waypoint
print 'Moving towards waypoint'
motion = self.dead_reckoning.pose.pose_to_pose_motion(
to_pose=target_pose, time_seconds=0.01)
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
if isinstance(self.active_subtask, ExitTask):
print 'self.active_subtask is an ExitTask, setting to None'
self.active_subtask = None
else:
print 'Polling subtask, tick {}'.format(self.subtask_tick)
task_result = self.active_subtask.poll_task(
context=context, tick=self.subtask_tick)
self.subtask_tick += 1
if task_result is not None:
self.subtask_tick = 0
self.active_subtask = task_result
self.active_subtask.init_task(context=context)
if self.active_subtask is None:
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:
return ExitTask()
def _set_motion(self, motion, context):
"""
Using the motion limit traction control, apply the specified motion to the chassis.
"""
motion = self.motion_limit.limit_and_return(motion=motion)
wheel_speeds = context.chassis.get_wheel_speeds(motion=motion)
speeds = wheel_speeds.speeds
power = [
speeds[i] / context.chassis.wheels[i].max_speed
for i in range(0, 3)
]
context.arduino.set_motor_power(power[0] * self.max_power,
power[1] * self.max_power,
power[2] * self.max_power)
class ManualMotionTask(Task):
"""
Class enabling manual control of the robot from the joystick. Uses dead-reckoning for bearing lock, we don't
use the IMU at all in this version of the class, it was proving problematic in real-world conditions and the
dead-reckoning logic is surprisingly accurate and stable.
"""
ACCEL_TIME = 1.0
'Time to reach full speed from a standing start'
def __init__(self):
super(ManualMotionTask, self).__init__(task_name='Manual motion', requires_compass=False)
self.bearing_zero = None
self.max_trn = 0
self.max_rot = 0
self.dead_reckoning = None
self.pose_display_interval = IntervalCheck(interval=0.2)
self.pose_update_interval = IntervalCheck(interval=0.1)
self.rate_limit = None
':type : triangula.dynamics.RateLimit'
self.motion_limit = None
':type : triangula.dynamics.MotionLimit'
self.limit_mode = 0
def init_task(self, context):
# Maximum translation speed in mm/s
self.max_trn = context.chassis.get_max_translation_speed()
# Maximum rotation speed in radians/2
self.max_rot = context.chassis.get_max_rotation_speed()
self._set_relative_motion(context)
self.dead_reckoning = DeadReckoning(chassis=context.chassis, counts_per_revolution=3310)
self.motion_limit = MotionLimit(
linear_acceleration_limit=context.chassis.get_max_translation_speed() / ManualMotionTask.ACCEL_TIME,
angular_acceleration_limit=context.chassis.get_max_rotation_speed() / ManualMotionTask.ACCEL_TIME)
self.rate_limit = RateLimit(limit_function=RateLimit.fixed_rate_limit_function(1 / ManualMotionTask.ACCEL_TIME))
self.limit_mode = 0
def _set_absolute_motion(self, context):
"""
Lock motion to be compass relative, zero point (forwards) is the current bearing
"""
context.lcd.set_backlight(3, 10, 0)
self.bearing_zero = self.dead_reckoning.pose.orientation
def _set_relative_motion(self, context):
"""
Set motion to be relative to the robot's reference frame
"""
context.lcd.set_backlight(10, 3, 9)
self.bearing_zero = None
def poll_task(self, context, tick):
# Check joystick buttons to see if we need to change mode or reset anything
if context.button_pressed(SixAxis.BUTTON_TRIANGLE):
self._set_relative_motion(context)
elif context.button_pressed(SixAxis.BUTTON_SQUARE):
self._set_absolute_motion(context)
elif context.button_pressed(SixAxis.BUTTON_CIRCLE):
self.dead_reckoning.reset()
elif context.button_pressed(SixAxis.BUTTON_CROSS):
self.limit_mode = (self.limit_mode + 1) % 3
# Check to see whether the minimum interval between dead reckoning updates has passed
if self.pose_update_interval.should_run():
self.dead_reckoning.update_from_counts(context.arduino.get_encoder_values())
# Update the display if appropriate
if self.pose_display_interval.should_run():
pose = self.dead_reckoning.pose
mode_string = 'ABS'
if self.bearing_zero is None:
mode_string = 'REL'
if self.limit_mode == 1:
mode_string += '*'
elif self.limit_mode == 2:
mode_string += '+'
context.lcd.set_text(row1='x:{:7.0f}, b:{:3.0f}'.format(pose.position.x, degrees(pose.orientation)),
row2='y:{:7.0f}, {}'.format(pose.position.y, mode_string))
# Get a vector from the left hand analogue stick and scale it up to our
# maximum translation speed, this will mean we go as fast directly forward
# as possible when the stick is pushed fully forwards
translate = Vector2(
context.joystick.axes[0].corrected_value(),
context.joystick.axes[1].corrected_value()) * self.max_trn
':type : euclid.Vector2'
# If we're in absolute mode, rotate the translation vector appropriately
if self.bearing_zero is not None:
translate = rotate_vector(translate,
self.bearing_zero - self.dead_reckoning.pose.orientation)
# Get the rotation in radians per second from the right hand stick's X axis,
# scaling it to our maximum rotational speed. When standing still this means
# that full right on the right hand stick corresponds to maximum speed
# clockwise rotation.
rotate = context.joystick.axes[2].corrected_value() * self.max_rot
# Given the translation vector and rotation, use the chassis object to calculate
# the speeds required in revolutions per second for each wheel. We'll scale these by the
# wheel maximum speeds to get a range of -1.0 to 1.0
# This is a :class:`triangula.chassis.WheelSpeeds` containing the speeds and any
# scaling applied to bring the requested velocity within the range the chassis can
# actually perform.
motion = Motion(translation=translate, rotation=rotate)
if self.limit_mode == 2:
motion = self.motion_limit.limit_and_return(motion)
wheel_speeds = context.chassis.get_wheel_speeds(motion=motion)
speeds = wheel_speeds.speeds
# Send desired motor speed values over the I2C bus to the Arduino, which will
# then send the appropriate messages to the Syren10 controllers over its serial
# line as well as lighting up a neopixel ring to provide additional feedback
# and bling.
power = [speeds[i] / context.chassis.wheels[i].max_speed for i in range(0, 3)]
if self.limit_mode == 1:
power = self.rate_limit.limit_and_return(power)
context.arduino.set_motor_power(power[0], power[1], power[2])