from cozmo_fsm import *

class Lab3(StateMachineProgram):
    def start(self):
        self.name = "Lab3"
        # Landmark format is ( (x,y), orient )
        # orient is the orientation in the x-y plane, in degrees.
        landmarks = {
            2 : ((160,55),0),
            3 : ((160,-55),0),
            0 : ((-55,160),90),
            1 : ((55, 160),90)
                     }
        pf = ParticleFilter(robot,
                            landmarks = landmarks,
                            sensor_model = ArucoDistanceSensorModel(robot))
        self.particle_filter = pf
        super().start()
        v = ParticleViewer(robot)
        v.startThread()