Robots & Behaviours

A project activity for Girls Into Coding using a online version of the free open source Webots robot simulator.

Activity series

This activity session is part of a series on simulating robots, you can see links to all the activities in this series below!

Session 1: Intro to robot simulation

Session 2: Pick and place with a robot arm

Session 3: Robots & Behaviours

This session is designed to be fun! The idea is that we can follow it together online, but that we can be free to move at our own pace. We’re going to be doing some basic python programming in this activity. If you’re not too familiar with Python, don’t worry, you’ll be able to follow along :) !

The idea with this acitivity isn’t to become an ACE python programmer in 1 hour, but it’s just to get an appreciation of programming robots. So that you can use this knowledge for good in the future :)!

Activity 1: What exactly are robot behaviours?
Activity 2: How do we program robot behaviours?
Activity 3: Let’s get familiar with our robot
Activity 4: Let’s make our robot move using python functions
Activity 5: How the robot senses objects
Activity 6: Changing the robot’s behaviour
Activity 7: Making the robot respond to time instead of distance
Activity 8: Making the robot change its movement type
Activity 9: Creating a state machine
Activity 10: Changing the state machine behaviours

Resources

Here's some resources that may help with the activity

Python tutorials at W3 Schools

Python cheatsheet

Webots documentation

30 women in robotics you need to know about – 2019

What exactly are robot behaviours?

Activity #1

Let’s take a look at what robot behaviours are!
Navigate to the robotbenchmark website.
Click on the Start button next to the Pick and place activity
You should see the scene below

Have a look at the simulation at the top right hand side of the screen
How many behaviours can you see in the robot? Let’s discuss this together
Some example answers are below

How do we program robot behaviours?

Activity #2

Before we start to use the robot simulation, we’re going to take a look at how we program simple robot behaviours in python! We can use this method in any programming language. We’re going to be using if statements, functions and loops :)

We’ll be using ASCII Art to illustrate these concepts !! Because it’s a really simple and fun way to introduce the topic.

If you get stuck at all, a great python tutorial resource is here!

Let’s have a look at how to program robot behaviours together!

Let’s get familiar with our robot

Activity #3

We’ll be using the robotbenchmark website.
Click on the Start button next to the Wall Following activity
Click on Start programming this benchmark
You should see the scene below

First we’re going to get familiar with how we control our view of the 3D simulation. We need to find how many ultrasonic sensors there are! Let’s control the viewpoint to count how many ultrasonic sensors there are on the robot.

Use the mouse to move the viewpoint and count the number of sensors

Rotate: click on a 3D object in the scene with the left mouse button and drag the mouse pointer. The viewpoint will rotate around the object.
Translate: click in the scene with the right mouse button and drag the mouse pointer.
Zoom: click in the 3D scene and roll the mouse wheel. It is also possible to zoom by pressing the mouse wheel and dragging the mouse forward or backwards.
Tilt: press the mouse wheel over the 3D scene and drag the mouse to the left or to the right. Pressing simultaneously the left and right mouse buttons is equivalent to pressing the mouse wheel.

There are 16 ultrasonic sensors, 4 at the front of the robot and 4 at the back!

Let’s make our robot move using python functions

Activity #4

We’re still going to be using the Wall Following activity at the robotbenchmark website. So you can keep this open all the time!
We’re going to get our robot moving!!
When you’re ready to program the robot, right click on it, and select Edit controller

A window will pop up with python code in it! This is how we program our virtual robot.
Delete all the code in the robot controller window, by hightlighting it all (or pressing ctrl-A) and then pressing delete
Copy and paste the ‘Activity Base Code’ code from below (click the green button) into the robot controller window, by hightlighting it all then right clicking and pressing copy (or pressing ctrl+C)

Start the simulation by following the instrucitons in the image above
You should see the robot approach the wall, but then reverse quickly when it senses it!
Have a look at the python functions that are making the robot move in the ‘Start of main program’ section of the code. We’ll discuss these together!

"""
Purpose: Sample base code controller for the 'finite state machine' girls into coding activity

Notes: Hi! we're going to be editing this program together to make the robot move. It's important 
that this is a fun activity! If anything is unclear please ask! We're really happy to help. A lot
of the code is already written to make this activity possible. The parts of the code that you 
should edit are the parts betweeen the squigly lines! like this...

# ~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*
# ~~~~ MAKE YOUR EDITS BELOW HERE ~~~
# ~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*

# some edits that we make together

# ~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*
# ~~~~ END OF YOUR CODE EDITS ~~~
# ~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*
"""

#---------------------
# Python library imports
#---------------------

from controller import Robot # imports the language of the robot ! 

#---------------------
# Starting up the robot
#---------------------

# Get pointer to the robot. So that we can talk to it!
robot = Robot()

# Get pointer to the robot wheels motors.
leftWheel = robot.getMotor('left wheel')
rightWheel = robot.getMotor('right wheel')

# We will use the velocity parameter of the wheels, so we need to
# set the target position to infinity. This means they will keep turning
# for ever unless we set their velocity to zero
leftWheel.setPosition(float('inf'))
rightWheel.setPosition(float('inf'))

# Put all the ultrasonic sensors in an array
sensors = []
sensors.append(robot.getDistanceSensor("so0"))
sensors.append(robot.getDistanceSensor("so1"))
sensors.append(robot.getDistanceSensor("so2"))
sensors.append(robot.getDistanceSensor("so3"))
sensors.append(robot.getDistanceSensor("so4"))
sensors.append(robot.getDistanceSensor("so5"))
sensors.append(robot.getDistanceSensor("so6"))
sensors.append(robot.getDistanceSensor("so7"))

# Get the time step of the current world (the smallest time unit)
timestep = int(robot.getBasicTimeStep())

# Switch all the sensors on
for sensor in sensors:
    sensor.enable(timestep)


#---------------------
# Helpful functions for controling the robot (for the girls into coding activity)
#---------------------

def stopRobotWheels():
    """
    Purpose: stop the robot
    Notes: mySpeed -> can take values from 1-9
    """
    leftWheel.setVelocity(0.0)
    rightWheel.setVelocity(0.0)
        
        
def startMoveForward(mySpeed):
    """
    Purpose: move the robot forward
    Notes: mySpeed -> can take values from 1-9
    """
    leftWheel.setVelocity(mySpeed)
    rightWheel.setVelocity(mySpeed)
    
def startMoveBackward(mySpeed):
    """
    Purpose: move the robot backward
    Notes: mySpeed -> can take values from 1-9
    """
    leftWheel.setVelocity(-mySpeed)
    rightWheel.setVelocity(-mySpeed)
    
def startTurnLeft(mySpeed):
    """
    Purpose: turn the robot left
    Notes: mySpeed -> can take values from 1-9
    """
    leftWheel.setVelocity(-mySpeed)
    rightWheel.setVelocity(mySpeed)
    
def startTurnRight(mySpeed):
    """
    Purpose: turn the robot right
    Notes: mySpeed -> can take values from 1-9
    """
    leftWheel.setVelocity(mySpeed)
    rightWheel.setVelocity(-mySpeed)
    
    

def getClosestObjectToRobot():
    """
    Purpose: Return the direction and distance for the closest object to the robot
    
    Notes: Refer to the diagrams on the girls into coding webpage for the distance
    convention. The distance is returned in meters. The value returned by the getValue() 
    method of the distance sensors corresponds to a physical value (here we have a sonar, 
    so it is the strength of the sonar ray). This function makes a conversion to a
    distance value in meters.
    """
    
    # Make local variables
    maxSensorRange = 1.6 # in meters
    currentSensorID = -1
    lowestSensorDistance = maxSensorRange
    lowestSensorID = None
    
    # Find the closest object to the robot and save the direction and distance (in m)
    for sensor in sensors:
        # calculate the distance in m (this formula is provided by the sensor manufacturer)
        currentSensorDistance = ((1000 - sensor.getValue()) / 1000) * 5
        currentSensorID = currentSensorID + 1
        
        if currentSensorDistance < lowestSensorDistance:
            lowestSensorDistance = currentSensorDistance
            lowestSensorID = currentSensorID
    
    # If no object is found, then return 'None' otherwise return the value       
    if lowestSensorID != None:
        currentDirection = lowestSensorID -3
    else:
        currentDirection = None
        lowestSensorDistance = None
        
    return currentDirection, lowestSensorDistance

#---------------------
# Start of main program
#---------------------

# Our starting message for the program
print("--------------")
print("Here is an example of how we write a message to the console")
print("Program starting !!")
print("--------------")


# Move forward 
startMoveForward(5)
# Keep going until we are 20cm away from the wall
while True:
    robot.step(500) # keep going for 0.5 seconds (value is in milli seconds (ms))
    direction, distance = getClosestObjectToRobot() # Get the info on the closest object
    print("direction = {}, distance = {}").format(direction, distance)  # Print the info on the closest object
    # If there is an object too close, then stop moving forward!
    if distance != None and distance < 0.2:
        break
    

# ~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*
# ~~~~ MAKE YOUR EDITS BELOW HERE ~~~
# ~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*
    
    
print("hello :)")
    
# Move backward 
startMoveBackward(5)
# Keep going until we are 1m away from the wall
while True:
    robot.step(500)
    direction, distance = getClosestObjectToRobot()
    print("direction = {}, distance = {}").format(direction, distance) 
    # If the object is too far, then stop moving backward!
    if distance != None and distance > 1:
        break
    
    
# ~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*
# ~~~~ END OF YOUR CODE EDITS ~~~
# ~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*
    
stopRobotWheels()

Let’s see how the robot is sensing objects

Activity #5

We’re still going to be using the Wall Following activity
Let’s have a look at how the robot is sensing the wall
Open up the console by following the instructions in the image below, then run the simulation
The console is where the robot prints messages to us!
Let’s discuss what it is saying
Every 0.5 seconds we are asking the robot to tell us the distance of any objects from it’s ultrasonic sensors
It is telling us the distance by printing this to the console
The diagram below shows how they work!

Questions Let’s answer these questions as a group

Can you see the moment at which the robot detects the distance to the wall?
How does the robot detect the wall?
What should the robot do when it detects a wall?

Try having a look at the function getClosestObjectToRobot() to see what it does!

The robot detects the wall when it is 1.5 meters away
The robot detects the wall using the ultrasonic sensors
When the robot detects the wall we need it to change behaviour (e.g. stop, or turn, or move backwards!)

Let’s change the robot’s behaviour

Activity #6

What would we do if we wanted to change the behaviour of the robot when it reaches the wall?
Let’s alter the code together to make this happen! Our goal is to make the robot move backwards, but not as far.
Open the ‘controller’ (code editor) for the robot

Scroll to the section of the code which looks like this (it should be lines 168-189):

# ~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*
# ~~~~ MAKE YOUR EDITS BELOW HERE ~~~
# ~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*
    
    
print("hello :)")
    
# Move backward 
startMoveBackward(5)
# Keep going until we are 1m away from the wall
while True:
    robot.step(500)
    direction, distance = getClosestObjectToRobot()
    print("direction = {}, distance = {}").format(direction, distance) 
    # If the object is too far, then stop moving backward!
    if distance != None and distance > 1:
        break
    
    
# ~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*
# ~~~~ END OF YOUR CODE EDITS ~~~
# ~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*

Let’s discuss this code together.
How would we change the code so that when the robot changes direction (from fowards to backwards) then it prints our name e.g. “hello there …”?
How would we make the robot move backwards less far?
Take a look at the hint and example answers if you’re stuck!

For printing to the console, try having a look at the code line: print(“hello :)”)

For the movement, try having a look at the “if statement”!

# ~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*
# ~~~~ MAKE YOUR EDITS BELOW HERE ~~~
# ~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*
    
    
print("hello there Nikita")
    
# Move backward 
startMoveBackward(5)
# Keep going until we are 1m away from the wall
while True:
    robot.step(500)
    direction, distance = getClosestObjectToRobot()
    print("direction = {}, distance = {}").format(direction, distance) 
    # If the object is too far, then stop moving backward!
    # We change this value to 0.5 so the robot travels on 0.5m backwards from the wall
    if distance != None and distance > 0.75:
        break
    
    
# ~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*
# ~~~~ END OF YOUR CODE EDITS ~~~
# ~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*

Making the robot respond to time instead of distance

Activity #7

What would we do if we wanted to change the movement so the robot changes behaviour after a time period?
Let’s alter the code together to make this happen! Our goal is to make the robot stop moving backwards after 2 seconds has elapsed
Open the ‘controller’ (code editor) for the robot

Scroll to the section of the code which looks like this (it should be lines 168-189):

# ~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*
# ~~~~ MAKE YOUR EDITS BELOW HERE ~~~
# ~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*
    
    
print("hello there Nikita")
    
# Move backward 
startMoveBackward(5)
# Keep going until we are 1m away from the wall
while True:
    robot.step(500)
    direction, distance = getClosestObjectToRobot()
    print("direction = {}, distance = {}").format(direction, distance) 
    # If the object is too far, then stop moving backward!
    # We change this value to 0.5 so the robot travels on 0.5m backwards from the wall
    if distance != None and distance > 0.75:
        break
    
    
# ~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*
# ~~~~ END OF YOUR CODE EDITS ~~~
# ~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*

Let’s discuss this code together.
How would we change the code so that robot stops after 2 seconds has elapsed
Have a look at the hint to make a start!

Look at the line “robot.step(500)”. This is the number of milliseconds that passes each time the robot moves. If we count these milliseconds we can change the IF statement!

# ~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*
# ~~~~ MAKE YOUR EDITS BELOW HERE ~~~
# ~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*
    
    
print("hello there Nikita")
    
# Move backward 
startMoveBackward(5)
# Keep going for 2 seconds
milliSeconds = 0
while True:
    robot.step(500)
    milliSeconds = milliSeconds + 500
    direction, distance = getClosestObjectToRobot()
    print("time = {}").format(milliSeconds) 
    # If the time is longer than 2 seconds, then stop moving!
    if milliSeconds > 2000:
        break
    
# ~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*
# ~~~~ END OF YOUR CODE EDITS ~~~
# ~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*

Avoiding the walls!

Activity #8

What would we do if we wanted to change the movement of the robot so that it starts to avoid walls?
Let’s alter the code together to make this happen! Our goal is to make the robot turn after it has moved backwards
Open the ‘controller’ (code editor) for the robot
Scroll to the section of the code which looks like this (it should be lines 168-189):

# ~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*
# ~~~~ MAKE YOUR EDITS BELOW HERE ~~~
# ~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*
    
    
print("hello there Nikita")
    
# Move backward 
startMoveBackward(5)
# Keep going for 2 seconds
milliSeconds = 0
while True:
    robot.step(500)
    milliSeconds = milliSeconds + 500
    direction, distance = getClosestObjectToRobot()
    print("time = {}").format(milliSeconds) 
    # If the time is longer than 2 seconds, then stop moving!
    if milliSeconds > 2000:
        break
    
    
# ~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*
# ~~~~ END OF YOUR CODE EDITS ~~~
# ~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*

Let’s discuss this code together.
How would we change the code so that robot turns when it has finished going backwards?
Have a look at the hint to make a start!

Try having a look at the function called ‘startTurnRight(mySpeed)’. How would you add this to the code to make the robot turn right.

# ~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*
# ~~~~ MAKE YOUR EDITS BELOW HERE ~~~
# ~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*
    
        
print("hello there Nikita")
    
# Move backward 
startMoveBackward(5)
# Keep going for 2 seconds
milliSeconds = 0
while True:
    robot.step(500)
    milliSeconds = milliSeconds + 500
    direction, distance = getClosestObjectToRobot()
    print("time = {}").format(milliSeconds) 
    # If the time is longer than 2 seconds, then stop moving!
    if milliSeconds > 2000:
        break

# Turn 
startTurnRight(5)
# Keep going for 2 seconds
milliSeconds = 0
while True:
    robot.step(500)
    milliSeconds = milliSeconds + 500
    print("time = {}").format(milliSeconds) 
    # If the time is too long, then stop moving!
    if milliSeconds > 2000:
        break
    
    
# ~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*
# ~~~~ END OF YOUR CODE EDITS ~~~
# ~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*

State machines

Activity #9

Our goal is to make the robot behaviour so that it randomly explores the map!
To keep going for ever it will need to avoid walls, and keep repeating behaviours
We could do this with loops! But it can get a bit complicated.
A really powerful idea for robot behaviours are state machines
It might sound scary but it’s really not!! Let’s take a look at what a state machine is together !

As we can see in the images, we just have some robot behaviours, and we call them states!
We’ve already created these robot behaviours, so there’s not much more to do
Let’s have a look at one kind of program structure for a state machine in python
Here is some pseudo code which shows how it works

So let’s implement this for the robot simulation together!
Delete all the code in the robot controller window, by hightlighting it all (or pressing ctrl-A) and then pressing delete
Copy and paste the ‘State Machine Code’ code from below (click the green button) into the robot controller window, by hightlighting it all then right clicking and pressing copy (or pressing ctrl+C)

Start the simulation by following the instrucitons in the image above
You should see the robot have the behaviours we programmed in the state machine
Have a look at the python functions that are making the robot move in the ‘Our States’ and ‘Our State Machine’ section of the code

"""
Purpose: State machine controller for the 'finite state machine' girls into coding activity

Notes: Hi! we're going to be editing this program together to make the robot move. It's important 
that this is a fun activity! If anything is unclear please ask! We're really happy to help. A lot
of the code is already written to make this activity possible. The parts of the code that you 
should edit are the parts betweeen the squigly lines! like this...

# ~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*
# ~~~~ MAKE YOUR EDITS BELOW HERE ~~~
# ~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*

# some edits that we make together

# ~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*
# ~~~~ END OF YOUR CODE EDITS ~~~
# ~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*
"""

#---------------------
# Python library imports
#---------------------

from controller import Robot # imports the language of the robot ! 

#---------------------
# Starting up the robot
#---------------------

# Get pointer to the robot. So that we can talk to it!
robot = Robot()

# Get pointer to the robot wheels motors.
leftWheel = robot.getMotor('left wheel')
rightWheel = robot.getMotor('right wheel')

# We will use the velocity parameter of the wheels, so we need to
# set the target position to infinity. This means they will keep turning
# for ever unless we set their velocity to zero
leftWheel.setPosition(float('inf'))
rightWheel.setPosition(float('inf'))

# Put all the ultrasonic sensors in an array
sensors = []
sensors.append(robot.getDistanceSensor("so0"))
sensors.append(robot.getDistanceSensor("so1"))
sensors.append(robot.getDistanceSensor("so2"))
sensors.append(robot.getDistanceSensor("so3"))
sensors.append(robot.getDistanceSensor("so4"))
sensors.append(robot.getDistanceSensor("so5"))
sensors.append(robot.getDistanceSensor("so6"))
sensors.append(robot.getDistanceSensor("so7"))

# Get the time step of the current world (the smallest time unit)
timestep = int(robot.getBasicTimeStep())

# Switch all the sensors on
for sensor in sensors:
  sensor.enable(timestep)


#---------------------
# Helpful functions for controling the robot (for the girls into coding activity)
#---------------------

def stopRobotWheels():
  """
  Purpose: stop the robot
  Notes: mySpeed -> can take values from 1-9
  """
  leftWheel.setVelocity(0.0)
  rightWheel.setVelocity(0.0)
      
      
def startMoveForward(mySpeed):
  """
  Purpose: move the robot forward
  Notes: mySpeed -> can take values from 1-9
  """
  leftWheel.setVelocity(mySpeed)
  rightWheel.setVelocity(mySpeed)
  
def startMoveBackward(mySpeed):
  """
  Purpose: move the robot backward
  Notes: mySpeed -> can take values from 1-9
  """
  leftWheel.setVelocity(-mySpeed)
  rightWheel.setVelocity(-mySpeed)
  
def startTurnLeft(mySpeed):
  """
  Purpose: turn the robot left
  Notes: mySpeed -> can take values from 1-9
  """
  leftWheel.setVelocity(-mySpeed)
  rightWheel.setVelocity(mySpeed)
  
def startTurnRight(mySpeed):
  """
  Purpose: turn the robot right
  Notes: mySpeed -> can take values from 1-9
  """
  leftWheel.setVelocity(mySpeed)
  rightWheel.setVelocity(-mySpeed)
  
def getClosestObjectToRobot():
  """
  Purpose: Return the direction and distance for the closest object to the robot
  
  Notes: Refer to the diagrams on the girls into coding webpage for the distance
  convention. The distance is returned in meters. The value returned by the getValue() 
  method of the distance sensors corresponds to a physical value (here we have a sonar, 
  so it is the strength of the sonar ray). This function makes a conversion to a
  distance value in meters.
  """
  
  # Make local variables
  maxSensorRange = 1.6 # in meters
  currentSensorID = -1
  lowestSensorDistance = maxSensorRange
  lowestSensorID = None
  
  # Find the closest object to the robot and save the direction and distance (in m)
  for sensor in sensors:
      # calculate the distance in m (this formula is provided by the sensor manufacturer)
      currentSensorDistance = ((1000 - sensor.getValue()) / 1000) * 5
      currentSensorID = currentSensorID + 1
      
      if currentSensorDistance < lowestSensorDistance:
          lowestSensorDistance = currentSensorDistance
          lowestSensorID = currentSensorID
  
  # If no object is found, then return 'None' otherwise return the value       
  if lowestSensorID != None:
      currentDirection = lowestSensorID -3.5
  else:
      currentDirection = None
      lowestSensorDistance = None
      
  return currentDirection, lowestSensorDistance


#---------------------
# Our States
#---------------------   
  
def goForwardsToWall():
  """
  Purpose: make the robot reach the wall and then change state
  Notes: robot stops 20cm from the wall
  """
  global robotState
  # Move forward 
  startMoveForward(5)
  # Keep going until we are 20cm away from the wall
  while True:
      robot.step(500) # keep going for 0.5 seconds (value is in milli seconds (ms))
      direction, distance = getClosestObjectToRobot() # Get the info on the closest object
      print("direction = {}, distance = {}").format(direction, distance)  # Print the info on the closest object
      # If there is an object too close, then stop moving forward!
      if distance != None and distance < 0.2:
          break
  robotState = 1
  
  

def goBackwardsFromWall():
  """
  Purpose: make the robot move away from the wall and then change state
  Notes: robot stops 1m from the wall
  """
  global robotState
  # Move backward 
  startMoveBackward(5)
  # Keep going until we are 1m away from the wall
  while True:
      robot.step(500)
      direction, distance = getClosestObjectToRobot()
      print("direction = {}, distance = {}").format(direction, distance) 
      # If the object is too far, then stop moving backward!
      if distance != None and distance > 1:
          break
  robotState = 2
  
  
def robotSpin():
  """
  Purpose: make the robot spin around 1 time :)
  Notes: you need to edit this function to make it work
  """
  # we want the robot to do a spin but it should end up facing the wall !
  
  global robotState
  # Start spining with speed = 5
  startTurnRight(5)
  # Keep going for 2 seconds
  timePassed = 0
  while True:
      robot.step(500) # 500 milli seconds
      timePassed = timePassed + 500
      direction, distance = getClosestObjectToRobot()
      print("direction = {}, distance = {}").format(direction, distance)
      print("time passed = {}").format(timePassed)
      
      # ~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*
      # ~~~~ MAKE YOUR EDITS BELOW HERE ~~~
      # ~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*
      
      # If the the time is too big, then stop spinning!
      if timePassed > 2000: # 2 seconds!
          break

      # ~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*
      # ~~~~ END OF YOUR CODE EDITS ~~~
      # ~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*
      
  robotState = 0

#---------------------
# Our State Machine
#--------------------- 

# The robot state is passed to the switcher object
switcher = {
  0: goForwardsToWall,
  1: goBackwardsFromWall,
  2: robotSpin
  }

# Create our state machine
def ourStateMachine(robotState):

  # Defines error message if incorrect state is requested
  func = switcher.get(robotState, "Invalid State") 
  func()


#---------------------
# Start of main program
#---------------------

# Our starting message for the program
print("--------------")
print("Here is an example of how we write a message to the console")
print("Program starting !!")
print("--------------")

robotState = 0
NumberMovementsCount = 0

# ~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*
# ~~~~ MAKE YOUR EDITS BELOW HERE ~~~
# ~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*

  
while True:
    ourStateMachine(robotState)
    print("robotState = {}").format(robotState)
  
# ~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*
# ~~~~ END OF YOUR CODE EDITS ~~~
# ~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*
  
stopRobotWheels()

Changing the state machine behaviours

Activity #10

The state machine allows us to very easily change the behaviour of the robot!
Let’s change the behaviour to make a state machine that does the following
(1) Go towards wall (2) Make a 360 degree spin (3) Go backwards away from wall (4) Repeat
How would we change the code to do this?
First let’s make the spin happen!
Have a look at the robotSpin() function, you can see this below
How can you change the time value to make the robot spin 360 degree (one complete turn?)

def robotSpin():
  """
  Purpose: make the robot spin around 1 time :)
  Notes: you need to edit this function to make it work
  """
  # we want the robot to do a spin but it should end up facing the wall !
  
  global robotState
  # Start spining with speed = 5
  startTurnRight(5)
  # Keep going for 2 seconds
  timePassed = 0
  while True:
      robot.step(500) # 500 milli seconds
      timePassed = timePassed + 500
      direction, distance = getClosestObjectToRobot()
      print("direction = {}, distance = {}").format(direction, distance)
      print("time passed = {}").format(timePassed)
      
      # ~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*
      # ~~~~ MAKE YOUR EDITS BELOW HERE ~~~
      # ~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*
      
      # If the the time is too big, then stop spinning!
      if timePassed > 2000: # 2 seconds!
          break

      # ~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*
      # ~~~~ END OF YOUR CODE EDITS ~~~
      # ~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*
      
  robotState = 0

Now lets look at the state machine
How can we change the transition conditions to change the order of behaviours?
What we want to achieve is shown in the image below
Notice the change in arrow directions !!

"""
Purpose: State machine controller for the 'finite state machine' girls into coding activity

Notes: Hi! we're going to be editing this program together to make the robot move. It's important 
that this is a fun activity! If anything is unclear please ask! We're really happy to help. A lot
of the code is already written to make this activity possible. The parts of the code that you 
should edit are the parts betweeen the squigly lines! like this...

# ~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*
# ~~~~ MAKE YOUR EDITS BELOW HERE ~~~
# ~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*

# some edits that we make together

# ~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*
# ~~~~ END OF YOUR CODE EDITS ~~~
# ~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*
"""

#---------------------
# Python library imports
#---------------------

from controller import Robot # imports the language of the robot ! 

#---------------------
# Starting up the robot
#---------------------

# Get pointer to the robot. So that we can talk to it!
robot = Robot()

# Get pointer to the robot wheels motors.
leftWheel = robot.getMotor('left wheel')
rightWheel = robot.getMotor('right wheel')

# We will use the velocity parameter of the wheels, so we need to
# set the target position to infinity. This means they will keep turning
# for ever unless we set their velocity to zero
leftWheel.setPosition(float('inf'))
rightWheel.setPosition(float('inf'))

# Put all the ultrasonic sensors in an array
sensors = []
sensors.append(robot.getDistanceSensor("so0"))
sensors.append(robot.getDistanceSensor("so1"))
sensors.append(robot.getDistanceSensor("so2"))
sensors.append(robot.getDistanceSensor("so3"))
sensors.append(robot.getDistanceSensor("so4"))
sensors.append(robot.getDistanceSensor("so5"))
sensors.append(robot.getDistanceSensor("so6"))
sensors.append(robot.getDistanceSensor("so7"))

# Get the time step of the current world (the smallest time unit)
timestep = int(robot.getBasicTimeStep())

# Switch all the sensors on
for sensor in sensors:
  sensor.enable(timestep)


#---------------------
# Helpful functions for controling the robot (for the girls into coding activity)
#---------------------

def stopRobotWheels():
  """
  Purpose: stop the robot
  Notes: mySpeed -> can take values from 1-9
  """
  leftWheel.setVelocity(0.0)
  rightWheel.setVelocity(0.0)
      
      
def startMoveForward(mySpeed):
  """
  Purpose: move the robot forward
  Notes: mySpeed -> can take values from 1-9
  """
  leftWheel.setVelocity(mySpeed)
  rightWheel.setVelocity(mySpeed)
  
def startMoveBackward(mySpeed):
  """
  Purpose: move the robot backward
  Notes: mySpeed -> can take values from 1-9
  """
  leftWheel.setVelocity(-mySpeed)
  rightWheel.setVelocity(-mySpeed)
  
def startTurnLeft(mySpeed):
  """
  Purpose: turn the robot left
  Notes: mySpeed -> can take values from 1-9
  """
  leftWheel.setVelocity(-mySpeed)
  rightWheel.setVelocity(mySpeed)
  
def startTurnRight(mySpeed):
  """
  Purpose: turn the robot right
  Notes: mySpeed -> can take values from 1-9
  """
  leftWheel.setVelocity(mySpeed)
  rightWheel.setVelocity(-mySpeed)
  
def getClosestObjectToRobot():
  """
  Purpose: Return the direction and distance for the closest object to the robot
  
  Notes: Refer to the diagrams on the girls into coding webpage for the distance
  convention. The distance is returned in meters. The value returned by the getValue() 
  method of the distance sensors corresponds to a physical value (here we have a sonar, 
  so it is the strength of the sonar ray). This function makes a conversion to a
  distance value in meters.
  """
  
  # Make local variables
  maxSensorRange = 1.6 # in meters
  currentSensorID = -1
  lowestSensorDistance = maxSensorRange
  lowestSensorID = None
  
  # Find the closest object to the robot and save the direction and distance (in m)
  for sensor in sensors:
      # calculate the distance in m (this formula is provided by the sensor manufacturer)
      currentSensorDistance = ((1000 - sensor.getValue()) / 1000) * 5
      currentSensorID = currentSensorID + 1
      
      if currentSensorDistance < lowestSensorDistance:
          lowestSensorDistance = currentSensorDistance
          lowestSensorID = currentSensorID
  
  # If no object is found, then return 'None' otherwise return the value       
  if lowestSensorID != None:
      currentDirection = lowestSensorID -3.5
  else:
      currentDirection = None
      lowestSensorDistance = None
      
  return currentDirection, lowestSensorDistance


#---------------------
# Our States
#---------------------   
  
def goForwardsToWall():
  """
  Purpose: make the robot reach the wall and then change state
  Notes: robot stops 20cm from the wall
  """
  global robotState
  # Move forward 
  startMoveForward(5)
  # Keep going until we are 20cm away from the wall
  while True:
      robot.step(500) # keep going for 0.5 seconds (value is in milli seconds (ms))
      direction, distance = getClosestObjectToRobot() # Get the info on the closest object
      print("direction = {}, distance = {}").format(direction, distance)  # Print the info on the closest object
      # If there is an object too close, then stop moving forward!
      if distance != None and distance < 0.2:
          break
  robotState = 2
  
  

def goBackwardsFromWall():
  """
  Purpose: make the robot move away from the wall and then change state
  Notes: robot stops 1m from the wall
  """
  global robotState
  # Move backward 
  startMoveBackward(5)
  # Keep going until we are 1m away from the wall
  while True:
      robot.step(500)
      direction, distance = getClosestObjectToRobot()
      print("direction = {}, distance = {}").format(direction, distance) 
      # If the object is too far, then stop moving backward!
      if distance != None and distance > 1:
          break
  robotState = 0
  
  
def robotSpin():
  """
  Purpose: make the robot spin around 1 time :)
  Notes: you need to edit this function to make it work
  """
  # we want the robot to do a spin but it should end up facing the wall !
  
  global robotState
  # Start spining with speed = 5
  startTurnRight(5)
  # Keep going for 2 seconds
  timePassed = 0
  while True:
      robot.step(500) # 500 milli seconds
      timePassed = timePassed + 500
      direction, distance = getClosestObjectToRobot()
      print("direction = {}, distance = {}").format(direction, distance)
      print("time passed = {}").format(timePassed)
      
      # ~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*
      # ~~~~ MAKE YOUR EDITS BELOW HERE ~~~
      # ~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*
      
      # If the the time is too big, then stop spinning!
      if timePassed > 1500: # 2 seconds!
          break

      # ~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*
      # ~~~~ END OF YOUR CODE EDITS ~~~
      # ~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*
      
  robotState = 1

#---------------------
# Our State Machine
#--------------------- 

# The robot state is passed to the switcher object
switcher = {
  0: goForwardsToWall,
  1: goBackwardsFromWall,
  2: robotSpin
  }

# Create our state machine
def ourStateMachine(robotState):

  # Defines error message if incorrect state is requested
  func = switcher.get(robotState, "Invalid State") 
  func()


#---------------------
# Start of main program
#---------------------

# Our starting message for the program
print("--------------")
print("Here is an example of how we write a message to the console")
print("Program starting !!")
print("--------------")

robotState = 0
NumberMovementsCount = 0

# ~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*
# ~~~~ MAKE YOUR EDITS BELOW HERE ~~~
# ~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*

  
while True:
    ourStateMachine(robotState)
    print("robotState = {}").format(robotState)
  
# ~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*
# ~~~~ END OF YOUR CODE EDITS ~~~
# ~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*
  
stopRobotWheels()