Thymio-II interface: Differential kinematics

The latest upgrade to my Thymio interface has been differential kinematics. Basically this is turning the wheels at different speeds in order to move in an arc. The Thymio has 2 wheels with separate motors for each so the wheels can move at different speeds very easily and so this form of motion can be implemented.

I decided to implement this using 2 variables, the radius of the arc and the distance traveled. With this information i can calculate the time the Thymio should move for and the speed of the wheels.

Previously i gave the user the ability to set the wheel speed so we can make use of that in the calculations. To calculate the individual speed we can do the following: set the outer most wheel to move at the wheel speed set by the player, calculate the difference between the length of the arcs of each wheel and then multiply the wheel speed by that difference.

In this example lets say the radius is 10cm and the length to travel is also 10cm. Looking up the specs of the Thymio we find that the width of it is 11cm, and as we want to use the middle of the wheels we can say that the difference is 10cm. The Thymio should move about 3cm/s at a speed of 100.

Calculating the time to move can be done in the following way. First we need to calculate theta (the angle we will me moving) by doing the length traveled divided by the radius, this will give us the angle in radians and works because finding the circumference of a circle  equals 2*pi*radius. That basically means that the 360 degrees (the full circle) is equal to 2*pi. to simplify this means that the angle equals the length divided by the radius. Now we know the angle we'll be traveling  we need to know the speed the center of the robot will travel. Theta * the radius (radius to the center) divided by theta * (radius + 5) will give us the difference between the 2 arcs. We can then times this number by the wheel speed to get the speed at the center and then we can divide this by 33.33 (speed of 100 = 3cm/s so speed/33.33 = speed in cm/s) to get the speed in cm per seconds. finally we can divide by the speed and the do 1 over this result to get the time to wait for the action to finish.
In our example this means theta is 1 and the difference will be 0.66 recurring and if the speed is 100 then the time we will need to wait will be 4.999 seconds

1/(((10/15 * 100 )/33.33333)/10)

Calculating the speed is very similar but instead of the radius +5 and the radius, we will use the radius-5. with the outer wheel moving at "wheelspeed" and the inner wheel moving at wheel speed * the difference.

That was quite a block of text but the result is that the center of the Thymio (the pen holder) will move the required distance in an arc.

The video below shows an example of this in action.

In the video i made use of a pi to run the python code. In the code there is a line which defines the IP address of the scratch serve the code should communicate with, by setting  this to the IP address of my laptop running the scratch code I can control it remotely.

To get the code running i used tight vnc to look at the Raspberry Pi's desktop remotely. I started the remote viewing using putty to communicate with the pi over ssh, I had to delete the .Xauthority in order to run the python as there was a conflict with running dbus. the file can be deleted by typing the following:

cd ~

sudo rm .Xauthority

Unfortunately this needs to be done every time you start up vnc on the pi

Thymio and Scratch using Python

Previously I managed to communicate between Python and the Thymio-II using dbus and asebamedulla, using that code I have been able to communicate between Scratch and the Thymio using python. As before this will only work on Linux due to dbus and asebamedulla compatibility.

Here is a video of everything working:

Scratch 1.4 allows for RSC (remote sensor connections) which means it acts as a sever of sorts and can deal with broadcasts and sensor values. As I understand it this was intended for some Lego robot but with python we can make good use of it. I will be following on from my previous post for those looking to follow along, one thing to note, this isn't especially clean, I only started using python a few days ago.

All of the python and the scratch files will be available for download at the bottom, they might be worth looking at for reference to help follow what I'm saying as I'm explaining in the order it was done in and not a top to bottom order.

To communicate to scratch you need to use sockets which can be complicated, however there is a nice library that deals with the communication between python and scratch called scratchpy. Scratchpy can be installed using pip, you'll want to run sudo apt-get install python-pip and then type sudo pip install scratchpy.

Scratchpy makes things a lot easier, to set it up you need to add the following import

import scratchpy

and then this code will set up a connection to scratch

s = scratch.Scratch(host='localhost')

And that's in on the python side. Bear in mind that you will need to go in to scratch, click on the sensing tab and then right click on the sensor value block to bring up a menu, then click on "enable remote sensor connections". With that done everything will be set.

We want to be able to see the values from the proximity sensors on the Thymio in scratch, the following code will send 5 sensor values to scratch to set them up

 s.sensorupdate({'prox0': '0', 'prox1': '0','prox2': '0','prox3': '0','prox4': '0'})

If you were to run this code you would find that in scratch on the sensor value drop down list all of the prox variable are present. In order to update the values in scratch we will need to make use of the main loop from last time. remove all code related to key presses from that function and enter

 s.sensorupdate({'prox0': proxSensorsVal[0], 'prox1': proxSensorsVal[1],'prox2': proxSensorsVal[2],'prox3': proxSensorsVal[3],'prox4': proxSensorsVal[4]})
    s.broadcast("setprox")

This will update all of the prox values in scratch with the values from the Thymio but only with some code in scratch. I added a list called prox sensors and gave it 5 items, the code below sets the values of those item to the sensor values from the Thymio. Note how above I broadcast "setprox" and below it responds to that broadcast.

With that done we want to be able to tell the thymio what to do based upon those values using scratch. Scratch can broadcast messages which can be read internally in scratch and externally in python. As well as this scratchpy has a function for receiving broadcasts, however it cause the thread to wait for data to be received before moving on. If we put that code in our main loop all of the sensor data would stop. To fix this we need to create another thread to deal with receiving and then to pass the data from scratch to the main loop and then to the thymio. We will need to define a global variable so data can be shared between threads.

broad = ' '

This just sets it up so it can be used later. Then we need the function for the main thread to run.

def thymioscratchcontrol():
    while True:
        res = s.receive()
        for r in res:
            if r == 'forward':
                global broad
                broad = r
                time.sleep(1)
                broad = 'null'
            if r == 'left':
                global broad
                broad = r
                time.sleep(0.2)
                broad = 'null'
            if r == 'right':
                global broad
                broad = r
                time.sleep(0.2)
                broad = 'null'
            if r == 'back':
                global broad
                broad = r
                time.sleep(1)
                broad = 'null'
            print r

This loops constantly and waits for a signal from the thymio. It then loops through every value it receives. the values are the names that are broadcast from scratch. If the name is one we expect we set the global variable to that name and then let the thread sleep for as long as we want the command to execute for.

To run this you will need to import thread and then run this code before you call loop.run().

thread.start_new_thread(thymioscratchcontrol,())

this will create the thread and run the method, all we need to do now is send the commands to the thymio. In the control loop add the following code.

if broad == "forward":
        network.SetVariable("thymio-II", "motor.left.target", [300])
            network.SetVariable("thymio-II", "motor.right.target", [300])
    elif broad == "left":
        network.SetVariable("thymio-II", "motor.left.target", [-300])
            network.SetVariable("thymio-II", "motor.right.target", [300])
    elif broad == "right":
        network.SetVariable("thymio-II", "motor.left.target", [300])
            network.SetVariable("thymio-II", "motor.right.target", [-300])
    elif broad == "back":
        network.SetVariable("thymio-II", "motor.left.target", [-300])
            network.SetVariable("thymio-II", "motor.right.target", [-300])
    else:
            network.SetVariable("thymio-II", "motor.left.target", [0])
            network.SetVariable("thymio-II", "motor.right.target", [0])

This will set the motor speed for the Thymio depending on the broadcast received, and the loop in the other thread determines how long to run these for. This just about covers all of the python, the .py can be downloaded at the bottom of this post.

In order to send commands you can use the broadcast block, at this point code like shown below should work.

This sends a list of commands, the python code will loop through this and send the commands to the thymio.

To use the sensors wee need to be a bit more careful so that it doesn't send 2 messages at the same time for instance, or create a huge queue in the python code which will clog up the thymio for a few minutes.

To do this we will need a variable to act as a flag for when it's okay to send new commands. I called the variable catch, and used it in the code below. This waits until the middle proximity sensor has a value greater than 1000 and then triggers.

This isn't all though, we need to increase catch by an appropriate amount. Luckily its broadcasting so we can just use those messages.

the amount catch is increased by is the same as the length of the command so when catch counts down to 0 the commands should have finished running. To get catch to count down we need some more code.

This will reduce catch by 0.2 every 0.2 seconds and ensure it's never smaller than 0.

All of this with my other post should allow you to use scratch to control the Thymio-II robot.

Some final thoughts, it should be possible to run asebamedulla and the python code on a raspberry pi using a battery and wifi and connect ot to scratch on a computer on the network, but I don't have the equipment and it might be slow due to the constant updating of the proximity sensors.

Downloads: python    Scratch

GitHub: https://github.com/lazerduck/Thymio_python_interface

Thymio II control with python

As part of my final year project I've been working with the Thymio II robot in an attempt to integrate it with Scratch. After significant searching I have basically figured it out and for the sake of recording it I'll write what I did here.

Here is a video of the robot in action:

First, this only works on Linux so far as there is very limited support for dbus (the communication middle-ware) on windows and it seems due to this there isn't a version of asebamedulla. this software allows for multiple connections to the thymio as well as connections using dbus.

You will need to install the aseba studio in order to get asebamedulla, this can be done with the usual apt-get install aseba. with this done asebamedulla can be run. with the thymio plugged in you need to type sudo asebamedulla "ser:name=Thymio-II". If done right it will find the thymio, say what port its on and then continue to run.

At this point we're ready for the python to communicate with it. The dbus libraries I have found, work best with python2.7 so when installing python I would recommend using that version. Dbus on python might come pre installed but if not then apt-get dbus as well as this you will need gobject for python.

create a file and open and prepare to write some python

first of all we're going to want to sort out our imports, for this we'll be using:

import dbus
import dbus.mainloop.glib
import gobject
import sys
import time
from optparse import OptionParser

Dbus will allow python to communicate with the thymio through asebamedulla.
Gojbect is used to run and handle the loop.
Sys will be used to receive key presses.
Time will be used to make python wait while an operation takes place.

Wow we need to define the function we want to loop for the thymio.

def control():
    #get the values of the sensors
    network.GetVariable("thymio-II", "prox.horizontal", reply_handler = get_variables_reply , error_handler = get_variables_error)
 
    #print the proximity sensors value in the terminal
    print proxSensorsVal[0], proxSensorsVal[1], proxSensorsVal[2], proxSensorsVal[3], proxSensorsVal[4]
 
    #read in a key press
    char = sys.stdin.read(1)
    #if a key is pressed
    if char == 'w':
       network.SetVariable("thymio-II", "motor.left.target", [300])
       network.SetVariable("thymio-II", "motor.right.target", [300])
       time.sleep(1)
       network.SetVariable("thymio-II", "motor.left.target", [0])
       network.SetVariable("thymio-II", "motor.right.target", [0])
    if char == 's':
       network.SetVariable("thymio-II", "motor.left.target", [-300])
       network.SetVariable("thymio-II", "motor.right.target", [-300])
       time.sleep(1)
       network.SetVariable("thymio-II", "motor.left.target", [0])
       network.SetVariable("thymio-II", "motor.right.target", [0])
    if char == 'a':
       network.SetVariable("thymio-II", "motor.left.target", [-300])
       network.SetVariable("thymio-II", "motor.right.target", [300])
       time.sleep(0.2)
       network.SetVariable("thymio-II", "motor.left.target", [0])
       network.SetVariable("thymio-II", "motor.right.target", [0])
    if char == 'd':
       network.SetVariable("thymio-II", "motor.left.target", [300])
       network.SetVariable("thymio-II", "motor.right.target", [-300])
       time.sleep(0.2)
       network.SetVariable("thymio-II", "motor.left.target", [0])
       network.SetVariable("thymio-II", "motor.right.target", [0])
    return True

we will also need to define the get_variable_reply and get_variable_error

def get_variables_reply(r):
    global proxSensorsVal
    proxSensorsVal=r
 
def get_variables_error(e):
    print 'error:'
    print str(e)
    loop.quit()

The above code will wait for a key press, when you press w, a, s, or d and then enter it will tell the thymio what to set the motor speeds to and then wait and set the speeds back to 0. Commands can also be queued as the sys.stfin.read(1) will only read one key at a time, for instance [waawsdds] would move the thymio forward, then turn it and then move forward again, and then reverse the process back to where it started.

All we need to do now is to actually create the network and the loop for the code to run in. To do this we will use the following

if __name__ == '__main__':
    parser = OptionParser()
    parser.add_option("-s", "--system", action="store_true", dest="system", default=False,help="use the system bus instead of the session bus")
 
    (options, args) = parser.parse_args()
 
    dbus.mainloop.glib.DBusGMainLoop(set_as_default=True)
 
    if options.system:
        bus = dbus.SystemBus()
    else:
        bus = dbus.SessionBus()
 
    #Create Aseba network
    network = dbus.Interface(bus.get_object('ch.epfl.mobots.Aseba', '/'), dbus_interface='ch.epfl.mobots.AsebaNetwork')
 
    #print in the terminal the name of each Aseba NOde
    print network.GetNodesList()  
    #GObject loop
    #print 'starting loop'
    loop = gobject.MainLoop()
    #call the callback of Braitenberg algorithm
    handle = gobject.timeout_add (100, Control) #every 0.1 sec
    loop.run()

Here we create a parser, the options variable and the bus variable. with these we can define the network. The 'ch.epfl.mobots.Aseba' is the name on asebamedulla for the code to connect to. After this we define the loop, tell it what to run and run it.

the full script is available here