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

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