summaryrefslogtreecommitdiffhomepage
path: root/host/simu/robots/marcel/model/loader.py
blob: 8968a968a3b841bbb8281bd5595f427c46ae51bf (plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
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
52
53
54
55
56
57
58
59
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
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
# simu - Robot simulation. {{{
#
# Copyright (C) 2010 Nicolas Schodet
#
# APBTeam:
#        Web: http://apbteam.org/
#      Email: team AT apbteam DOT org
#
# This program is free software; you can redistribute it and/or modify
# it under the terms of the GNU General Public License as published by
# the Free Software Foundation; either version 2 of the License, or
# (at your option) any later version.
#
# This program is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
# GNU General Public License for more details.
#
# You should have received a copy of the GNU General Public License
# along with this program; if not, write to the Free Software
# Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
#
# }}}
"""Marcel food loader."""
from utils.observable import Observable
from simu.utils.trans_matrix import TransMatrix
from math import pi

class Loader (Observable):

    CLAMP_PULLEY_RADIUS = 6.5
    CLAMP_WIDTH = 198
    CLAMP_LENGTH = 75

    ELEVATOR_PULLEY_RADIUS = 11.15
    ELEVATOR_LINEAR_STROKE = 32
    ELEVATOR_ROTATING_STROKE = 200 - 32

    FRONT_ZONE_X_MIN = 120 + 15
    FRONT_ZONE_X_MAX = 120 + CLAMP_LENGTH - 15

    def __init__ (self, table, robot_position, left_clamp_link, right_clamp_link,
            elevator_link):
        Observable.__init__ (self)
        self.table = table
        self.robot_position = robot_position
        self.clamp_link = (left_clamp_link, right_clamp_link)
        self.elevator_link = elevator_link
        self.clamp_load = [ ]
        self.load = [ ]
        self.clamp_pos = [ None, None ]
        self.clamp_link[0].register (self.__left_clamp_notified)
        self.clamp_link[1].register (self.__right_clamp_notified)
        self.elevator_link.register (self.__elevator_notified)

    def __elevator_notified (self):
        if self.elevator_link.angle is None:
            self.elevator_height = None
            self.elevator_angle = None
        else:
            # Update elevator position.
            self.elevator_height = (self.elevator_link.angle
                    * self.ELEVATOR_PULLEY_RADIUS)
            if self.elevator_height < self.ELEVATOR_LINEAR_STROKE:
                self.elevator_angle = 0
            else:
                self.elevator_angle = ((self.elevator_height -
                    self.ELEVATOR_LINEAR_STROKE)
                    / self.ELEVATOR_ROTATING_STROKE * pi)
        self.notify ()

    def __clamp_notified (self, clamp):
        if self.clamp_link[clamp].angle is None:
            self.clamp_pos[clamp] = None
        else:
            # Update clamp position.
            self.clamp_pos[clamp] = (self.clamp_link[clamp].angle
                    * self.CLAMP_PULLEY_RADIUS)
            # If elevator is low, pick elements.
            if (not self.clamp_load
                    and self.elevator_height < self.ELEVATOR_LINEAR_STROKE):
                elements = self.__get_front_elements ()
                if elements:
                    black = sum (hasattr (e, 'black') and e.black for e in
                            elements)
                    tickness = sum (e.radius * 2 for e in elements)
                    if (not black and self.CLAMP_WIDTH - sum (self.clamp_pos)
                            <= tickness):
                        for e in elements:
                            e.pos = None
                            e.notify ()
                        self.clamp_load = elements
            # If elevator is high, drop elements.
            if (self.clamp_load
                    and self.elevator_height > self.ELEVATOR_LINEAR_STROKE +
                    self.ELEVATOR_ROTATING_STROKE * 2 / 3):
                tickness = sum (e.radius * 2 for e in self.clamp_load)
                if self.CLAMP_WIDTH - sum (self.clamp_pos) > tickness:
                    self.load += self.clamp_load
                    self.clamp_load = [ ]
        self.notify ()

    def __left_clamp_notified (self):
        self.__clamp_notified (0)

    def __right_clamp_notified (self):
        self.__clamp_notified (1)

    def __get_front_elements (self):
        """Return a list of elements in front of the robot, between clamp."""
        elements = [ ]
        if (self.robot_position is None
                or self.clamp_pos[0] is None
                or self.clamp_pos[1] is None):
            return None
        # Matrix to transform an obstacle position into robot coordinates.
        m = TransMatrix ()
        m.translate ((-self.robot_position.pos[0],
            -self.robot_position.pos[1]))
        m.rotate (-self.robot_position.angle)
        # Look up elements.
        # This could be used if clamp blocking is handled or elements are
        # pushed:
        #ymin = -self.CLAMP_WIDTH / 2 + self.clamp_pos[1]
        #ymax = self.CLAMP_WIDTH / 2 - self.clamp_pos[0]
        ymin = -self.CLAMP_WIDTH + 40
        ymax = self.CLAMP_WIDTH - 40
        for o in self.table.obstacles:
            if o.level == 1 and o.pos is not None:
                pos = m.apply (o.pos)
                if (pos[0] > self.FRONT_ZONE_X_MIN
                        and pos[0] < self.FRONT_ZONE_X_MAX
                        and pos[1] > ymin and pos[1] < ymax):
                    elements.append (o)
        return elements