PYTHON   30

# LoveHate

Guest on 2nd August 2022 12:43:47 AM

1. from cozmo_fsm import *
2. import math
3.
4. #adapted from 15-494 lecture nodes
5. #Checks if either of the 2 cubes are in view
6. #Posts the cube's number if it is
7. class CubeFinder(StateNode):
8.     def start(self, event=None):
9.         if self.running:return
10.         super().start(event)
11.
12.         if cube1.is_visible:
13.             self.post_data(1)
14.         if cube2.is_visible:
15.             self.post_data(2)
16.         else:
17.             self.post_data(0)
18.
19. class LoveTurn(Turn):
20.       def start(self, event=None):
21.           cY = cube1.pose.position.y
22.           cX = cube1.pose.position.x
23.           rY =  robot.pose.position.y
24.           rX = robot.pose.position.x
26.           self.angle = Angle(atan2((cY -rY), (cX - rX)) - rPose)
27.           super().start(event)
28.
29.
30. class HateTurn(Turn):
31.       def start(self, event=None):
32.           cY = cube2.pose.position.y
33.           cX = cube2.pose.position.x
34.           rY =  robot.pose.position.y
35.           rX = robot.pose.position.x
37.           self.angle = Angle(atan2((cY -rY), (cX - rX)) - rPose - pi)
38.           super().start(event)
39.
40.
41. class LoveHate(StateMachineProgram):
42.     \$setup {
43.
44.
45.         #Deal with seeing cube 1
46.         look =D(1)=> {cube1act, loveit}
47.
48.         look: CubeFinder()
49.         cube1act: LoveTurn()
50.         loveit: Say('love it')
51.
52.         {cube1act, loveit} =C=> look
53.
54.         #Deal with seeing cube 2
55.         look =D(2)=> {cube2act, hateit}
56.
57.         cube2act: HateTurn()
58.         hateit: Say('hate it')
59.
60.         {cube2act, hateit} =C=> look
61.
62.
63.         #Deal with seeing cube 0
64.         look =D(0)=> look
65.     }