Skip to content

pid_position_test

AngularPositionTestAction #

Bases: Action

Action used to move the robot without table. First set start position on 0x0. Them rotate of 180° in the same position. Then go back to start position. Do it in loop.

Source code in cogip/tools/planner/actions/pid_position_test.py 
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
class AngularPositionTestAction(Action):
    """
    Action used to move the robot without table.
    First set start position on 0x0.
    Them rotate of 180° in the same position.
    Then go back to start position.
    Do it in loop.
    """

    def __init__(self, planner: "Planner", strategy: Strategy):
        super().__init__("AngularPositionTest action", planner, strategy)
        self.angular_distance = 180
        self.linear_speed = 66
        self.angular_speed = 66
        self.motion_direction = MotionDirection.BIDIRECTIONAL
        self.before_action_func = self.init_start_position
        self.pose_init = models.Pose(
            x=-500,
            y=-300,
            O=-90,
        )
        self.pose_start = Pose(**self.pose_init.model_dump())
        self.pose_start.after_pose_func = partial(self.append_pose, self.pose_start)
        self.pose_end = self.pose_start.model_copy(update={"O": self.pose_start.O + self.angular_distance})
        self.pose_end.after_pose_func = partial(self.append_pose, self.pose_end)
        self.poses.append(self.pose_end)
        self.poses.append(self.pose_start)

    async def init_start_position(self):
        await self.planner.set_pose_start(self.pose_init)
        self.planner.pose_reached = False
        self.planner.action = self

    async def append_pose(self, pose: Pose) -> None:
        self.poses.append(pose)

    def weight(self) -> float:
        return 1000000.0

LinearPositionTestAction #

Bases: Action

Action used to move the robot without table. First set start position on 0x0. Them move straight forward along 100 cm. Then go back to start position. Do it in loop.

Source code in cogip/tools/planner/actions/pid_position_test.py 
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
class LinearPositionTestAction(Action):
    """
    Action used to move the robot without table.
    First set start position on 0x0.
    Them move straight forward along 100 cm.
    Then go back to start position.
    Do it in loop.
    """

    def __init__(self, planner: "Planner", strategy: Strategy):
        super().__init__("LinearPositionTest action", planner, strategy)
        self.distance = 750
        self.linear_speed = 66
        self.angular_speed = 66
        self.motion_direction = MotionDirection.BIDIRECTIONAL
        self.before_action_func = self.init_start_position
        self.pose_init = models.Pose(
            x=-500,
            y=-300,
            O=-90,
        )
        self.pose_start = Pose(**self.pose_init.model_dump())
        self.pose_start.after_pose_func = partial(self.append_pose, self.pose_start)
        self.pose_end = self.pose_start.model_copy(update={"y": self.pose_start.y - self.distance})
        self.pose_end.after_pose_func = partial(self.append_pose, self.pose_end)
        self.poses.append(self.pose_end)
        self.poses.append(self.pose_start)

    async def init_start_position(self):
        await self.planner.set_pose_start(self.pose_init)
        self.planner.pose_reached = False
        self.planner.action = self

    async def append_pose(self, pose: Pose) -> None:
        self.poses.append(pose)

    def weight(self) -> float:
        return 1000000.0