PYTHON   48

# TurnToClosestCube

Guest on 2nd August 2022 12:46:15 AM

1. from cozmo_fsm import *
2. from math import *
3.
4. #Runs the fsm to look at each cub and then turn towards the closest one
5. class TurnToClosestCube(StateMachineProgram):
6.     \$setup{
7.         #Turns Cozmo 360-degrees so he is able to get the position of all cubes
8.         #He needs to move very slowly in order to consistently see every cube
9.         scanForCubes: DriveTurn(360, 5)
10.         scanForCubes =C=> getClosest
11.         getClosest: FindClosestCube() =D=> turnTo
12.         #Goes back to StateNode at the end to prevent issues upon relaunching
13.         turnTo: TurnToFace() =C=> StateNode()
14.     }
15.
16. #Takes the position of each cube into account and posts the closest one
17. class FindClosestCube(StateNode):
18.     def start(self, event=None):
19.         if self.running:return
20.         super().start(event)
21.
22.         #Finds the distance to cube one
23.         x = cube1.pose.position.y- robot.pose.position.y
24.         y = cube1.pose.position.x- robot.pose.position.x
25.         hyp1 = sqrt(x**2 + y**2)
26.         #Sets to an arbitrary (far) distance if the cube has been moved
27.         if cube1.pose.origin_id != robot.pose.origin_id:
28.                 hyp1 = 1*10**9
29.
30.         #Finds the distance to cube 2
31.         x = cube2.pose.position.y- robot.pose.position.y
32.         y = cube2.pose.position.x- robot.pose.position.x
33.         hyp2 = sqrt(x**2 + y**2)
34.         #Sets to an arbitrary (far) distance if the cube has been moved
35.         if cube2.pose.origin_id != robot.pose.origin_id:
36.                 hyp2 = 1*10**9
37.
38.         #Finds the distance to cube 3
39.         x = cube3.pose.position.y- robot.pose.position.y
40.         y = cube3.pose.position.x- robot.pose.position.x
41.         hyp3 = sqrt(x**2 + y**2)
42.         #Sets to an arbitrary (far) distance if the cube has been moved
43.         if cube3.pose.origin_id != robot.pose.origin_id:
44.                 hyp3 = 1*10**9
45.
46.         #sets closest to the closest cube (or 0 if all of the cubes are...)
47.         if hyp1<hyp2 and hyp1<hyp3:
48.             closest = 1
49.         elif hyp2<hyp1 and hyp2<hyp3:
50.             closest = 2
51.         elif hyp3<hyp1 and hyp3<hyp2:
52.             closest=3
53.         elif hyp1==hyp2 and hyp2==hyp3 and hyp1==1*10**9:
54.             closest=0
55.         else:
56.             closest=2
57.
58.         self.post_data(closest)
59.
60. #Turns Cozmo to face the closest cube
61. class TurnToFace(Turn):
62.     # try it with only one of the super().start(event)
63.     def start(self, event=None):
64.         if self.running:return
65.
66.         #Finds the anflr that cozmo needs to turn in order to face the closest cube
67.         if isinstance(event, DataEvent):
68.             closest = event.data
69.             if closest ==1:
70.                 x = cube1.pose.position.y- robot.pose.position.y
71.                 y = cube1.pose.position.x- robot.pose.position.x
73.             elif closest ==2:
74.                 x = cube2.pose.position.y- robot.pose.position.y
75.                 y = cube2.pose.position.x- robot.pose.position.x
77.             elif closest==3:
78.                 x = cube3.pose.position.y- robot.pose.position.y
79.                 y = cube3.pose.position.x- robot.pose.position.x