As described in a recent post, our robot now has an Infra-Red distance sensor, which allows us to measure the distance from the front of the robot to the nearest obstacle. With respect to the autonomous navigation code that we wish to write, this will be the cornerstone! In order to evalute the perfomance of the sensor, we surrounded the robot with obstacles and commanded it to rotate slowly in an anti-clockwise direction, while IR data was collected 20 times per second:

Surrounded by obstacles, C3Pi rotates anti-clockwise and returns IR distance data every 0.05 seconds

Collecting the Data

We initialized the workspace by loading first the “RainPro” graphics package (which is included with Dyalog APL on the Pi), and then the robot code:

    )load rainpro                   Loads the graphics workspace
    ]load /home/pi/DyaBot           Loads the robot control code

The following function loops 300 times (once every 0.05 seconds), repeatedly collecting the value of the robot’s IRange property (which contains the current distance measured by the IR sensor). The call to the UpdateIRange method of the bot ensures that a fresh sample has just been taken (otherwise, the robot will update the value automatically every 100ms).

      ∇ r←CollectIRangeData bot;i;rc
 [1]    r←⍬
 [2]    :For i :In ⍳300
 [3]        :If 0=1⊃rc←bot.UpdateIRange
 [4]            r←r,bot.Irange
 [5]            ⎕DL 0.05 ⍝ Wait 1/20 sec
 [6]        :Else
 [7]            ∘∘∘ ⍝ Intentional error if update fails
 [8]        :EndIf
 [9]    :EndFor
      ∇

We can now perform the experiment as follows:

      iBot←⎕NEW DyaBot ⍬           ⍝ Instance of the robot class
      iBot.Speed←40 0              ⍝ 40% power on right wheel only 
      ⍴r←CollectIRangeData iBot    ⍝ Check shape of collected values 
300
      iBot.Speed←0                 ⍝ Let the robot rest its batteries
      1⍕10↑r                       ⍝ First 10 observations to 1 decimal 
15.3 12.8 13.7 12.7 13.7 11.4 9.9 11.1 10.4 9.4

Now that we have the data, the following function calls will create our first chart:

     ch.Set 'head' 'IR Sensor Data' ⍝ Set Chart Header
     ch.Set 'ycap' 'Distance (cm)'  ⍝     Y caption
     ch.Set 'xcap' 'Time (s)'       ⍝     X caption
     ch.Set 'xfactor'(÷0.05)        ⍝ Scale the x-axis to whole seconds
     ch.Plot data                   ⍝ Create the chart
     '/home/pi/irline.svg' svg.PS ch.Close ⍝ Render it to SVG

Removing the Noise with a Moving Average

The chart above suggests that the robot performed a complete rotation every 5 seconds or so with just under 100 observations per cycle. The signal seems quite noisy, so some very simple smoothing would probably make it easier to understand. The following APL function calculates a moving average for this purpose – it does this by creating moving sums with a window size given by the left argument, and dividing these sums by the window size:

       movavg←{(⍺ +/ ⍵) ÷ ⍺}  ⍝ Define the function 
       3 movavg 1 2 3 4 5     ⍝ Test it
 2 3 4

We can re-use the existing chart settings and plot smoothed data as follows:

      #.ch.Plot 7 movavg data      
      '/home/pi/irline.svg' svg.PS ch.Close

Making Sense of It

The pattern is now nice and clear – but how does the map compare to the territory? We can use a “Polar” chart of the distance to see how the measured distances compare to reality:

     ∇ filename PolarDistance data;⎕PATH;mat;deg;window;smoothed;angles;movavg
[1]   ⍝ Polar IR Distance plot - "data" is one cycle of observations
[2]   ⍝ Note the -ve right margin to get the chart off-centre!
[3]
[4]    ⎕PATH←'#.ch'      ⍝ Using RainPro ch namespace
[5]    window←7          ⍝ Smoothing window size
[6]    deg←⎕UCS 176      ⍝ Degree symbol
[7]    movavg←{(⍺+/⍵)÷⍺} ⍝ Moving average with window size ⍺
[8]
[9]    angles←360×(⍳⍴data)÷⍴data ⍝ All the way round
[10]   smoothed←window movavg data,(¯1+window)↑data
[11]   data←(⌈window÷2)⌽data     ⍝ Rotate data so centre of window is aligned with moving average
[12]
[13]   Set'head' 'Infra-Red;Distance;Measurement'
[14]   Set('hstyle' 'left')('mleft' 12)('mright' ¯60)
[15]   Set'footer' 'Distance measured;every 0.05 seconds;while 3Pi was rotating'
[16]   Set'style' 'lines,curves,xyplot,time,grid,hollow'
[17]   Set'lines' 'solid'
[18]   Set'nib' 'medium,broad'
[19]   Set('yr' 0 60)('ytick' 10)
[20]   Set('xr' 0 360)('xtick' 15)('xpic'('000',deg))
[21]   Set('key' 'Measured' '7 MovAvg')('ks' 'middle,left,vert')
[22]
[23]   Polar angles,data,⍪smoothed
[24]   filename svg.PS Close  
     ∇

To align the chart with the picture, we need to:

1. Extract the first 99 observations – corresponding to one rotation
2. Reverse the order of the data, because the robot was rotating anti-clockwise
3. Finally, rotate the data by 34 samples, to align the data with the photograph (the recording started with the robot in the position shown on the photograph)

We can do these three operations using the expression on the next line, and then pass this as an argument to the PolarDistance function, which creates another SVG file:

     onerotation←¯34⌽⌽99↑r
     '/home/pi/irpolar.svg' PolarDistance onerotation

If we compare the red line to the picture we started with, and take into account the fact that the robot was rotating quite fast and the IR sensor probably needs a little time to stabilise, it looks quite reasonable. The accuracy isn’t great, but with a little smoothing it does seem we should be able to stop little C3Pi from bumping into too many things!

Stay tuned for videos showing some autonomous driving, and the code to do it…

Follow

Back from the APL meeting in Hamburg, where my C3Pi made it’s first appearance on German soil (and a few days meeting with APL users in Milan). I’ve extended the control program which was used to make last week’s figure 8 video to give me some “hand controls”. I do appreciate that it is inadvisable for humans to control motor vehicles directly, but in the privacy of my own home I have risked a little careful driving:

The APL Code

You can see the latest code at https://github.com/APLPi/APLBot. However, beware that significant changes will be made next week – both to the APL code and the Arduino / I2C interface layers. We will be adding support for our first sensor – an Infra-Red sensor that will allow us to measure the distance to the nearest obstacle (in front of the ‘bot) – and this requires some extensions.

The APL code currently consists of three files: I2C.dyalog, DyaBot.dyalog and DrivePi.dyalog. These need to be placed in the same folder on your C3Pi.

The I2C.dyalog file relies on the library lib2c-com.so, which will be installed along with Dyalog APL for the Pi when this becomes available.

The final piece of the puzzle is the control program for the Arduino. That’s in APLPi/I2CArduinoComm. Install it by following the relevant part of Jason’s instructions on building your own C3Pi.

The three APL code files implement the following layers:

I2C: A namespace which loads libi2-com.so and makes four interface functions available in the active workspace. Strictly speaking, this file belongs in the libi2c-com repo, and will move there when I have time to co-ordinate rewriting some test cases with Liam.

DyaBot: A class, built upon I2C, which allows APL applications to control the robot by setting a field named “Speed” to a 2-element vector containing values between ¯100 (full reverse) and 100 (full speed ahead) for each of the 2 wheels.

DrivePi: A namespace, built upon DyaBot, containing a function Run which provides a very simple “game interface” for driving the C3Pi, and a function Play which accepts 3-column matrices of (Right Wheel Speed, Left Wheel Speed, Duration) records, which are “played back”.

What next?

The ultimate goal of our project is to write some code which allows the C3Pi to perform Simultaneous Localization and Mapping (SLAM): The robot should be able to examine its surroundings, build a map, and accept high-level instructions to move from point to point, avoiding obstructions. As mentioned above, we’ll be addding the first collision-avoidance sensor next week. Once that is done, I’ll be back to discuss the other sensors that we are planning to add to the C3Pi in the coming weeks. I hope that some of you will join us in building your own robots and helping write the software!

Follow