Example on how to use finites state machines on a Student Robotics robot.

sr_fsm.py

# -*- coding: utf-8 -*-

# Solution for Munich kickstart tasks 2015
# Introduces the vision system, servo control and a finate state machine
# ----------------------------------------------------------------------
#
# Loest die Aufgaben, die in den Zeilen stehen, die mit #TASK# beginnen.
# ----------------------------------------------------------------------

# import SR libraries
from sr.robot import *
import time

# Create the robot object
R = Robot()

# define states
SUCHE = 0
DREHE_TOKEN = 1
TOKEN_FERTIG = 2

# state variable
state = SUCHE               # SUCHE is initial state

# servo positions
SERVO_SHOW_STANDBY = -50
SERVO_SHOW_TURN = 50

# define variables
marker_orientation_z = 0    # orientation of marker
marker_distance = 0         # distance to marker

# dummy variable
DUMMY = None


# This is the main loop
while True:
            
    # state SUCHE
    if state == SUCHE:
        print "SUCHE"
        
        #TASK_servo# Fuegt hier einen Befehl ein, der euren Servo auf die Standby-Position dreht
        
        # Use vision system to scan markers
        #TASK# Speichert die Marker, die eure Kamera erkennen kann, in "markers" ab.
        markers = []    # Ersetzt "[]"
        print "I can see", len(markers), "markers"
        
        # extract orientation and distance of markers
        if len(markers) > 0:
            #TASK# Speichert die Orientierung des Markers um die z-Achse in die Variable "marker_orientation_z"
            marker_orientation_z = DUMMY
            #TASK# Speichert die Entfernung des Markers von der Kamera in die Variable "marker_distance"
            marker_distance = DUMMY
        
        # debug massage: print orientation and distance of marker
        if len(markers) > 0:
            print "Orientation: ", int(marker_orientation_z), "degree"
            print "Distance: ", int(marker_distance), "cm"
        
        # should we switch to DREHE_TOKEN?
        if len(markers) > 0:
            if -160 <= marker_orientation_z <= 160:
                if marker_distance < 1.0:
                    # Token can be turned
                    state = DREHE_TOKEN
        
        # should we switch to TOKEN_FERTIG?
        if len(markers) > 0:
            # Check the orientation and distance of the marker
            #TASK# Formuliert die Bedingung, die erfüllt sein muss um in den Zustand TOKEN_FERTIG zu wechseln
            if DUMMY:
                # Token has already the right orientation
                state = TOKEN_FERTIG
    
    
    # state DREHE_TOKEN
    elif state == DREHE_TOKEN:
        print "DREHE_TOKEN"
        
        #TASK_servo# Fuegt hier einen Befehl ein, der euren Servo auf die "Drehen"-Position dreht
        
        # Use the vision system to scan markers
        #TASK# Speichert die Marker, die eure Kamera erkennen kann, in "markers" ab.
        markers = []    # Ersetzt "[]"
        
        # extract orientation and distance of markers
        if len(markers) > 0:
            #TASK# Speichert die Orientierung des Markers um die z-Achse in die Variable "marker_orientation_z"
            marker_orientation_z = DUMMY
            #TASK# Speichert die Entfernung des Markers von der Kamera in die Variable "marker_distance"
            marker_distance = DUMMY
            
        # debug massage: print orientation and distance of marker
        if len(markers) > 0:
            print "Orientation: ", int(marker_orientation_z), "degree"
            print "Distance: ", int(marker_distance), "cm"
       
        # should we switch to ______?
        # Check whether the marker is still visible
        if len(markers) > 0:
            # Check the orientation of the marker
            if marker_orientation_z > 160 or marker_orientation_z < -160:
                #TASK# In welchen State soll der Roboter hier wechseln?
                state = DUMMY
        
        # should we switch to SUCHE?
        if len(markers) == 0:
            # Go back to the initial state
            state = SUCHE
        
    
    # state TOKEN_FERTIG
    elif state == TOKEN_FERTIG:
        print "TOKEN_FERTIG"
        #TASK_servo# Fuegt hier einen Befehl ein, der euren Servo auf die Standby-Position dreht
        
        # Use the vision system to scan markers
        #TASK# Speichert die Marker, die eure Kamera erkennen kann, in "markers" ab.
        markers = []    # Ersetzt "[]"
        
        # If no marker was scaned go to state SUCHE
        #TASK# Formuliert die Bedingung, die erfüllt sein muss um in den Zustand SUCHE zu wechseln
        if DUMMY:
            state = SUCHE
    
    # Undefined state
    else:
        print "Invalid state ", state