예제 #1
0
    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])
예제 #2
0
    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,
예제 #3
0
    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)))