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])
for translation in [
Vector2(x * 10, y * 10) for x in range(0, 6) for y in range(0, 6)
]:
final_pose = Pose().translate(translation).calculate_pose_change(
motion=motion, time_delta=time_delta)
translated_target = target_pose.translate(translation)
try:
assert final_pose.is_close_to(translated_target)
except AssertionError as e:
print 'Failed: {} not {}, translation {}, motion {}, time_delta {}'.format(
final_pose, translated_target, translation, motion, time_delta)
raise e
check(
Motion(translation=rotate_vector(Vector2(0, -100 * pi / 2), radians(45)),
rotation=radians(90)), 1.0,
Pose(position=Point2(-sqrt(2) * 100, 0), orientation=radians(90)))
check(Motion(rotation=radians(30)), 0.5, Pose(orientation=radians(15)))
check(Motion(translation=Vector2(0, 100 * pi / 2), rotation=radians(90)), 1.0,
Pose(position=Point2(100, 100), orientation=radians(90)))
check(Motion(translation=Vector2(0, 100 * pi / 2), rotation=radians(90)), 2.0,
Pose(position=Point2(200, 0), orientation=radians(180)))
check(Motion(translation=Vector2(0, 100 * pi / 2), rotation=radians(-90)), 2.0,
Pose(position=Point2(-200, 0), orientation=radians(180)))
check(Motion(translation=Vector2(0, 100 * pi), rotation=radians(180)), 0.5,
max_rotations_per_second=1.0)
def check(motion, time_delta, target_pose):
for translation in [Vector2(x * 10, y * 10) for x in range(0, 6) for y in range(0, 6)]:
final_pose = Pose().translate(translation).calculate_pose_change(motion=motion, time_delta=time_delta)
translated_target = target_pose.translate(translation)
try:
assert final_pose.is_close_to(translated_target)
except AssertionError as e:
print 'Failed: {} not {}, translation {}, motion {}, time_delta {}'.format(final_pose, translated_target,
translation, motion, time_delta)
raise e
check(Motion(translation=rotate_vector(Vector2(0, -100 * pi / 2), radians(45)), rotation=radians(90)),
1.0,
Pose(position=Point2(-sqrt(2) * 100, 0), orientation=radians(90)))
check(Motion(rotation=radians(30)),
0.5,
Pose(orientation=radians(15)))
check(Motion(translation=Vector2(0, 100 * pi / 2), rotation=radians(90)),
1.0,
Pose(position=Point2(100, 100), orientation=radians(90)))
check(Motion(translation=Vector2(0, 100 * pi / 2), rotation=radians(90)),
2.0,
Pose(position=Point2(200, 0), orientation=radians(180)))
