task3.py 文件源码

def start(self, _=None):
        """ Start the macro """
        anchor = self.fc.assure_anchor("center")
        if anchor is None:
            log('Click on the 2 farthest parts of the side of the table '
                'you want to approach')
            return False

        self.fc.zarj.walk.fix_stance()
        zarj.utils.wait_for_walk(self.fc.zarj)

        r_x = anchor.adjusted[0]
        r_y = anchor.adjusted[1]
        r_a = anchor.angle

        log('Table is {},{} at angle {}'.format(r_x, r_y, r_a))

        off_a = 90 - r_a
        off_x = sin(radians(off_a))*0.75
        off_y = cos(radians(off_a))*0.75

        tgt_x = r_x - off_x
        tgt_y = r_y + off_y

        log('Approach point is {},{}'.format(tgt_x, tgt_y))

        heading = degrees(atanh(tgt_y/tgt_x))
        distance = sqrt(tgt_x**2 + tgt_y**2)

        log('Path has a heading of {} and distance {}m'.format(heading,
                                                               distance))

        self.fc.zarj.walk.turn(heading, snap_to=0.0)
        zarj.utils.wait_for_walk(self.fc.zarj)
        if self.stop:
            return

        self.fc.zarj.walk.forward(distance)
        zarj.utils.wait_for_walk(self.fc.zarj)
        if self.stop:
            return

        turn = r_a + heading

        if abs(turn) > 45:
            self.fc.zarj.walk.turn(-turn/2.0, snap_to=0.0)
            zarj.utils.wait_for_walk(self.fc.zarj)
            if self.stop:
                return
            self.fc.zarj.walk.turn(-turn/2.0, snap_to=0.0)
        else:
            self.fc.zarj.walk.turn(-turn, snap_to=0.0)
        zarj.utils.wait_for_walk(self.fc.zarj)
        if self.stop:
            return

        self.fc.zarj.walk.forward(0.2)
        zarj.utils.wait_for_walk(self.fc.zarj)