PYTHON   42
FirstTurn
Guest on 1st February 2023 01:35:56 AM

1. from cozmo_fsm import *
2. from cozmo.util import degrees, Pose
3. import math
4.
5. #create the global variables for storing R
6. global currentR
7. global lastX
8. global lastY
9.
10. #Performs the first set of necessary operations
11. class FirstTurn(Turn):
12.     def start(self, event = None):
13.         global lastX, lastY
14.         #finds the initial X and Y positions of the robot
15.         lastX = robot.pose.position.x
16.         lastY = robot.pose.position.y
17.         #executes the first turn
18.         self.angle = Angle(pi)
19.         super().start(event)
20.
21. #Finds the first value of R
22. class InitializeR(StateNode):
23.
24.     def start(self, event=None):
25.         if self.running: return
26.         super().start(event)
27.         global lastX, lastY, currentR
28.         #Finds the robots positions after the first turn
29.         currX = robot.pose.position.x
30.         currY = robot.pose.position.y
31.         xdiff = currX - lastX
32.         ydiff = currY - lastY
33.         #Caluculates R and sets currentR to its value
34.         currentR = math.sqrt(ydiff**2 + xdiff**2)
35.         print(currentR)
36.         #Update lastX and lastY for the next iteration
37.         lastX = currX
38.         lastY = currY
39.         self.post_completion()
40.
41. #Updates the average value of R following each turn
42. class UpdateR(StateNode):
43.
44.     def start(self, event = None):
45.         if self.running: return
46.         super().start(event)
47.         global lastX, lastY, currentR
48.         #finds the robots new position
49.         currX = robot.pose.position.x
50.         currY = robot.pose.position.y
51.         xdiff = currX - lastX
52.         ydiff = currY - lastY
53.         #average the new R value into the current R value
54.         currentR = (currentR + math.sqrt(ydiff**2 + xdiff**2)) / 2
55.         #update lastX and lastY for the next iteration
56.         lastX = currX
57.         lastY = currY
58.         print(currentR)
59.         self.post_completion()
60.
61.
62. #Describes the FindR state machine
63. class FindR(StateMachineProgram):
64.     \$setup {
65.         StateNode() =N=> turn1
66.         turn1: FirstTurn() =C=> initialize
67.         initialize: InitializeR() =C=> next_turn
68.         next_turn: Turn(180) =C=> update
69.         update: UpdateR() =C=> next_turn
70.         }

Raw Paste

or to edit or fork this paste. It's free.