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
  25.           rPose = robot.pose.rotation.angle_z.radians
  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
  36.           rPose = robot.pose.rotation.angle_z.radians
  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.     }

Raw Paste


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