Average Ticker

AverageBot Devblog 10 – Code Breakdown

AverageBot CodeIt's time to talk about code...

This will be the final blog post in my AverageBot devblog series. I might have been able to fit this all in to Devblog #9 but I couldn’t sleep with it ending on an odd number! To see the previous devblogs, click here to go to my Pi Wars page.

I promised a lot of people that I’d explain my code at the end of the Devblog series, mostly because I knew it wouldn’t have been complete until the last minute.

I’ll explain my code in the same way I did for my Raspberry Pi Social Network Monitor project – lines and blocks of code one at a time. It takes a while and results in a very long blog post, but it’s really helpful when trying to learn, and also gives me something to refer back to in a few months when I’ve forgotten how to do certain things.

Grab a coffee folks…

Code Style and Functionality

It’s probably best to set a bit of context before I talk about the code itself.

First and foremost, my robot was coded in Python. I used a single Python script with everything in one place.

The robot was controlled using an iPazzPort 2.4Ghz media centre remote, which uses a USB adapter to receive signals. To get my remote key presses talking to my Raspberry Pi, I used PyGame (it’s not just for making games).

The script focusses around 7 main programs, one for each challenge, selected by a switch on my robot and the program number displayed on the IP display dongle that fits in my motor controller. These are as follows:

  1. General remote control
  2. Proximity alert (autonomous)
  3. 3-Point Turn (autonomous)
  4. Line Follower (autonomous)
  5. Skittles
  6. Command: Exit program
  7. Command: Shut down

The Code

Let’s have a look at AverageBot’s code and try to explain how it works.

Before you start reading, I’d really recommend downloading the script from my PasteBin first, as it makes it easier to follow when I chunk things up and skip over irrelevant or duplicate lines. Line numbers in this blog post, for each code chunk, should be accurate and match the downloaded script.

It’s also worth mentioning that there are some duplicated functions in my code – I could probably refine this a lot, but it works so I’m leaving it alone!

Lastly, I’ve removed initial indentation positions in the code you see in this blog post. It helps keep things readable, and you can refer to my PasteBin for proper indentation.

Intro Section

You can mostly ignore lines 1-28, as these are just information lines and a little ‘print’ to tell you that the script has started running.

Imports

As with most Python projects, I needed to import some things to make my robot work.

I start this block with a title line to split it out, and a ‘print’ to tell me that the imports section has started:

#======================================================================
# IMPORTS

print "- - - - - START IMPORTS"

I then start importing. First I import ‘os’ which is needed to allow me do a little trick later with PyGame:

# IMPORT OS FOR PYGAME SCREEN FRIG
import os
print "imported os"

Next is smbus. I import this as it’s needed to run the MCP23017 chip on the IP display board I’m using:

# IMPORT SBMUS FOR 4TRONIX DISPLAY BOARD
import smbus
print "imported smbus"

An old favourite next – GPIO – which is needed for the motor controller and anything connected to it:

# IMPORT GPIO FOR MOTOR CONTROLLER
import RPi.GPIO as GPIO
print "imported GPIO"

I then import ‘subprocess’ for operating system commands like ‘os.system(“sudo halt”)’ that we use later on:

# IMPORT SUBPROCESS FOR OS. COMMANDS
import subprocess
print "imported subprocess"

PyGame is next – we import PyGame to allow us to use the 2.4Ghz remote:

# IMPORT PYGAME FOR 2.4GHZ KEYBOARD CONTROL
import pygame
print "imported pygame"

Next we import ‘time’ – a very common import to allow time delays etc:

# IMPORT TIME FOR THE PAUSES IN OUR CODE
import time
print "imported time"

We then import ‘sys’ – I’m not 100% on what this exactly does, but it was included in the motor controller example code so probably best I keep it in here!

# IMPORT SYS FOR MOTOR CONTROLLER CODE
import sys
print "imported sys"

Lastly we import ‘threading’. Just like the import above, I’m not sure if I even use threading in this script, but it was in the example so it stays put. I should probably test these lines commented out to see if my robot still runs:

# IMPORT THREADING FOR MOTOR CONTROLLER CODE
import threading
print "imported threading"

That’s the imports done, let’s carry on with the rest of the setup lines.

Setting the GPIO Mode

We imported GPIO, which brings it to the party, but we need to tell it which mode we want to run – BCM or BOARD. It’s just the numbering convention for the pins on the Raspberry Pi, and it’s usually just a personal preference thing. BOARD is the physical pin numbering and BCM is the Broadcom numbering.

I’m using BOARD, as you can see below in my GPIO setup block of code:

#======================================================================
# SET GPIO MODE

print "- - - - - START GPIO SETMODE"
time.sleep(0.5)

GPIO.setmode(GPIO.BOARD) # SET GPIO MODE TO BOARD
print "GPIO setmode complete"

print ""

Setting up PyGame

As mentioned earlier, I use a media centre remote to control AverageBot. I don’t think Python treats these 2.4Ghz signals via USB as a standard connected keyboard, so we have to use PyGame to detect the presses and depresses.

We code that good stuff later on, but before we start the main program we need to set up PyGame to make sure it’s ready to go.

PyGame expects a screen to be connected, so we have to trick it to make it think one is connected. Pi Podcast presenter Albert Hickey showed me one way of doing this, using the ‘os.environ’ method you can see on line 84 below.

After that we initialise (start) PyGame (line 87), and then set up some fake screen credentials on lines 91-93:

#======================================================================
# PYGAME SETUP

print "- - - - - START PYGAME SETUP"
time.sleep(0.5)

os.environ["SDL_VIDEODRIVER"] = "dummy" # SET UP FAKE VIDEO DRIVER FOR PYGAME
print "PyGame - fake video driver set up"

pygame.init() # INITIALISE PYGAME
print "PyGame - initialised"

# SET UP FAKE SCREEN SETTINGS FOR PYGAME
width = 320
height = 240
display = pygame.display.set_mode((width,height))
print "PyGame - fake screen settings complete"

print ""

Setting up the Servos

I use servos in my ‘Big Pusha’ attachment that I made for the skittles challenge. Whilst the attachment was a bit of a failure in terms of getting skittles to fall over, the code worked well.

This is probably the simplest way of using a servo with the Raspberry Pi, and this is clear when you see the odd twitching that the servos produce when being controlled in this basic software fashion.

If you want to do something similar, I recommend watching this excellent YouTube video on using servos with the Raspberry Pi, which will explain this code a lot better than I can!

In lines 106 and 107 I set up the GPIO pin that each servo is connected to (2 servos = 2 GPIO pins), and then set the GPIO direction to OUTPUT in lines 110 and 111 (because we want to give the servos signals, not receive signals like you would with a sensor).

Lines 114-121 is where it gets tricky. This is where we set up the GPIOs as PWM (lines 114-115) and set a frequency, then in lines 120-121 I set the initial position of the servos by setting the desired duty cycle. Again, watch the video in the link above to learn all about this:

#======================================================================
# SET UP SERVOS

print "- - - - - START SERVO SETUP"
time.sleep(0.5)

# SET UP GPIO NUMBERS
servo1 = 15
servo2 = 16

# SET GPIO DIRECTIONS FOR SERVOS (OUT)
GPIO.setup(servo1, GPIO.OUT) # SET SERVO1 AS A GPIO OUTPUT
GPIO.setup(servo2, GPIO.OUT) # SET SERVO2 AS A GPIO OUTPUT
print "GPIO set up"

servo1 = GPIO.PWM(servo1,50) # SERVO1 PWM FREQUENCY SET TO 5OHZ
servo2 = GPIO.PWM(servo2,50) # SERVO2 PWM FREQUENCY SET TO 5OHZ
time.sleep(0.5) # BREAK TO ALLOW SERVOS TO SET
print "Servo frequency set"

# SET INITIAL SERVO POSITION
servo1.start(4) # SET SERVO 1 INTO INITIAL POSITION
servo2.start(11) # SET SERVO 2 INTO INITIAL POSITION
print "Servos set up"

print ""

Setting up the Switches

AverageBot uses a single switch to control the IP display. It’s a SPDT (Single Pole Double Throw) switch, which means it is off in the middle position, and can momentarily hit a different switch contact by pushing left or right.

I had the two switch positions set to a different GPIO pin, allowing me to hit it one way to change the number on the IP display (the program to run from 1-7), and then hit it the other way to select the program i had chosen. Clever huh?

In this initial setup I select the GPIO pins to be used for the switch (lines 134-135), then set the GPIOs as inputs with pull-down resistors in lines 139-140. Using these software pull-down resistors removes the need for physical resistors, which made my build that little bit tidier.

More info on software pull-downs and pull-ups can be found over at RasPi.TV:

#======================================================================
# SET UP SWITCHES

print "- - - - - START SWITCH SETUP"
time.sleep(0.5)

# SET UP GPIO NUMBERS
switch1 = 22
switch2 = 18
print "Switch GPIO numbers set"

# SET GPIO DIRECTIONS FOR SWITCHES (IN)
GPIO.setup(switch1,GPIO.IN, pull_up_down=GPIO.PUD_DOWN)
GPIO.setup(switch2,GPIO.IN, pull_up_down=GPIO.PUD_DOWN)
print "Swich directions set to inputs"

print ""

Setting up smbus

Smbus is needed to drive the MCP23017 chip on the IP display board, and something smbus needs you to do is tell it what revision of Raspberry Pi you are using. I’m using an original Model A on AverageBot which is revision 2.

You will see in line 153 that I have ‘1’ in the brackets. If I was using an older revision 1 Pi, I’d have a ‘0’:

#======================================================================
# SET UP SMBUS FOR 4TRONIX DISPLAY BOARD

print "- - - - - START SMBUS SETUP"
time.sleep(0.5)

# SET SMBUS NUMBER
bus = smbus.SMBus(1) # 0 for Revision 1 Pi
print "smbus ready"
   
print ""

Setting up the IP Display Board

I used one of these little 7-segment display boards on my robot as they are designed to fit directly on to the range of motor controller I was using. Whilst it’s intended to be used as an IP address display, you can of course change the code to show whatever you want.

I had a lot of headaches trying to get this to show numbers how I wanted as I’m generally a bit uneducated when it comes to binary, so I eventually settled for a simple number in the first segment showing the program number selected by my switch. It’s a visual way of choosing what to run on my robot without having to have a phone or laptop connected.

Lines 166-177 are the binary (hex) codes for the characters I wanted to show on the display. Think of this list as a library to refer to in code later on.

Lines 181-183 set up the MCP23017 chip that controls the segment display. It first sets the address (should always be 0x20 as this is how the board is wired) and then sets both banks of the chip as outputs (to output power to the segments on the display).

Lastly line 187 clears the display in case anything is showing from previous runs:

#======================================================================
# SET UP 4TRONIX DISPLAY BOARD

print "- - - - - START DISPLAY BOARD SETUP"
time.sleep(0.5)

# SET UP CHARACTERS FOR DISPLAY BOARD (HEX CODES)
N0 = 0x3F
N1 = 0x06
N2 = 0x5B
N3 = 0x4F
N4 = 0x66
N5 = 0x6D
N6 = 0x7D
N7 = 0x07
N8 = 0x7F
N9 = 0x6F
dash = 0x40
underscore = 0x08
print "characters ready"

# SET UP MCP23017 FOR DISPLAY BOARD
addr = 0x20 # I2C address of MCP23017
bus.write_byte_data(addr, 0x00, 0x00) # SET ALL OF BANK 0 TO OUTPUTS 
bus.write_byte_data(addr, 0x01, 0x00) # SET ALL OF BANK 1 TO OUTPUTS
print "MCP23017 ready"

# CLEAR DISPLAY
bus.write_byte_data(addr, 0x13, 0xff) # SET ALL OF BANK 1 TO HIGH (OFF)
print "display cleared"

print ""

Setting up the SR-04 Distance Sensor

AverageBot uses the very common SR-04 distance sensor to manage obstacle avoidance. It’s not the most accurate sensor but they’re cheap, simple and readily available.

Like most things, it uses a GPIO pin. Line 200 defines which pin the sensor will be using, however we don’t set up the pin direction yet. We do that later on in the code module that makes the sensor work:

#======================================================================
# SET UP SR-04 GPIO PINS

print "- - - - - START SR-04 SETUP"
time.sleep(0.5)

# SET SR-04 GPIO PIN
sonar = 8
print "SR-04 GPIO pin set"

print ""

Setting up the Motors

The PiRoCon controller on AverageBot uses GPIO pins to control the motors, and PWM on these pins to control the speed. Most of this section has been taken from the example code provided with the controller.

Line 213 “initialises the PWM device using the default address”…I can’t claim to know a lot about this. These letters are referenced in the next section though, so this step is necessary!

Lines 217-220 set up each motor with a left/right GPIO pin (each motor uses 2 GPIO pins). Lines 224-238 set each of these pins as an output, and then assign the letters above to them at the same time as setting the pins to use PWM.

Each motor also technically ‘starts’ at 0 PWM here as well, which is the same as being stopped:

#======================================================================
# SET UP MOTORS

print "- - - - - START MOTOR GPIO SETUP"
time.sleep(0.5)

# INITIALISE THE PWM DEVICE USING DEFAULT ADDRESS
global p, q, a, b 
print "PWM address set"

# MOTOR GPIO PINS
R1 = 24
R2 = 26
L1 = 19
L2 = 21
print "Motor GPIO pins set"

# USE PWM TO CONTROL MOTOR SPEED
GPIO.setup(L1, GPIO.OUT) # SET LEFT MOTOR GPIO 1 AS OUTPUT
p = GPIO.PWM(L1, 20) # SET LEFT MOTOR GPIO 1 AS PWM
p.start(0) # START LEFT MOTOR 1 PWM AT ZERO

GPIO.setup(L2, GPIO.OUT) # SET LEFT MOTOR GPIO 2 AS OUTPUT
q = GPIO.PWM(L2, 20) # SET LEFT MOTOR GPIO 2 AS PWM
q.start(0) # START LEFT MOTOR 2 PWM AT ZERO

GPIO.setup(R1, GPIO.OUT) # SET RIGHT MOTOR GPIO 1 AS OUTPUT
a = GPIO.PWM(R1, 20) # SET RIGHT MOTOR GPIO 1 AS PWM
a.start(0) # START RIGHT MOTOR 1 PWM AT ZERO

GPIO.setup(R2, GPIO.OUT) # SET RIGHT MOTOR GPIO 2 AS OUTPUT
b = GPIO.PWM(R2, 20) # SET RIGHT MOTOR GPIO 2 AS PWM
b.start(0) # START RIGHT MOTOR 2 PWM AT ZERO
print "Motor GPIO pins set"

print ""

Still with me? Maybe grab another coffee…

Set up Speed Levels

My motors are controlled using PWM, which allows each motor to be set a specific speed from 0-100. Unfortunately my tracks (or the hardware I used to mount them to my robot) are a little inaccurate, so I couldn’t simply set both motors to ’80’ and expect it to run straight.

I worked around this by testing over and over with different speeds until I found a setting that made it run straight. I then made three versions of this for slow, medium and fast. I also took a similar approach with turning speed.

This is why this setup section exists – to set these speed levels in one place, so that if I need to change them, I only have to alter this single section.

The code below should be mostly self explanatory after reading the paragraph above. The only extra part is in lines 280-284, which simply sets the starting speeds when the robot boots:

#======================================================================
# SET UP SPEED LEVELS
#    - This section sets a number of speed levels
#    - Allows us to change the PWM tuning for the entire program
#    - First number is the left motor, second is the right motor
#    - Motors have different values to compensate for inaccuracies

print "- - - - - START SPEED LEVEL SETUP"
time.sleep(0.5)

# TURN SPEED SETTINGS (PWM)
SpeedLow = 60
SpeedMedium = 80
SpeedHigh = 100
print "Turn speeds set"

# FORWARD SPEED SETTINGS (PWM)
ForwardLowL = 18
ForwardLowR = 20
ForwardMediumL = 57
ForwardMediumR = 60
ForwardHighL = 98
ForwardHighR = 99
print "Forward speeds set"

# REVERSE SPEED SETTINGS (PWM)
ReverseLowL = 26
ReverseLowR = 20
ReverseMediumL = 60
ReverseMediumR = 56
ReverseHighL = 98
ReverseHighR = 94
print "Reverse speeds set"

# SET INITIAL MOTOR SPEEDS

TurnSpeed = SpeedHigh         # Start @ SpeedHigh
ForwardSpeedL = ForwardHighL  # Start @ ForwardHighL
ForwardSpeedR = ForwardHighR  # Start @ ForwardHighR
ReverseSpeedL = ReverseHighL  # Start @ ReverseHighL
ReverseSpeedR = ReverseHighR  # Start @ ReverseHighR
print "Initial speeds set"

print ""

Set up the Line Sensors

For the line following challenge I fitted and wired three line sensors. Each one requires a GPIO pin set up as an input, and this is what we do here.

Lines 297-299 assign each sensor a GPIO pin, and lines 303-305 set each of these pins as inputs (as they will be receiving signals from the sensors):

#======================================================================
# SET UP LINE SENSORS

print "- - - - - START LINE SENSOR SETUP"
time.sleep(0.5)

# SET GPIO FOR LINE FOLLOWING SENSORS
lineRight = 11  #(BROWN)
lineMiddle = 12 # (WHITE)
lineLeft = 13  #(BLUE)
print "Line sensor GPIO numbers set"

# SET UP DIGITAL LINE DETECTORS AS INPUTS
GPIO.setup(lineRight,GPIO.IN)
GPIO.setup(lineMiddle,GPIO.IN)
GPIO.setup(lineLeft,GPIO.IN)
print "Line sensor GPIO direction set (IN)"

print ""

Set up Count and Initial IP Display Character

This is the final bit of setup before we start the main program. Here we set up a count to use later in the display menu code (line 318), and also set an underscore character on the display. This character is useful when you turn the robot on, as it indicates that the program is running and ready:

#======================================================================
# SET COUNT AND INITIAL DISPLAY CHARACTER

print "- - - - - START INITIAL COUNT & DISPLAY CHARACTER SETUP"
time.sleep(0.5)

# SET INITIAL COUNT FOR DISPLAY BOARD MENU CODE
count = 0
print "Count set to zero"

# SET AN UNDERSCORE IN THE MENU TO START WITH (MCP23017 COMMAND)
bus.write_byte_data(addr, 0x13, 7)
bus.write_byte_data(addr, 0x12, underscore)
print "Initial display character set"

print ""

Lines 329-350 are just comments and ‘prints’ for information more than anything, so I’ll skip over those.

Main Program – Try/Except Block

I should clarify where I’ve put my main program, as I’ve used something here that I haven’t tried before.

My main program is in a ‘try’ block. ‘try’ blocks should always come with an ‘except’ or ‘finally’ block as well.

What a ‘try’ block does is run your main program of code in the same way you normally would, however if there’s an error or you exit the program, this ‘try’ block will always run the ‘except’ or ‘finally’ block before closing down.

Why is this useful? Let’s take GPIO pins for example. If you shut down your script without running ‘GPIO.cleanup()’ your GPIO pins may remain in use which can cause conflicts on the next run.

Another good example is PyGame – you need to quit PyGame properly using ‘pygame.quit()’ or a number of PyGame gremlins will remain running in the background, causing issues if you try to run the script again.

Our ‘except’ or ‘finally’ block can contain all of these clean shut down actions, meaning every time you close the script or an error occurs, you can feel safe in the knowledge that everything is shut down properly ready for the next run.

Main Program – Modules

Something else I should clarify is the use of modules in my code – everything is in a module, and all of these modules sit within the ‘try’ block.

My description of a module is “a block of code that is given a name, that we can run just by entering that name in our code”.

Imagine you wanted to light 3 LEDs at different points in your program. Let’s say that your code looked like this (it’s an example, not any real programming language!):

Light LED 1
Light LED 2
Light LED 3

You could just type that out every time you needed them to light in your program, but it’s a lot easier to put them in a ‘module’ like this:

def LightThreeLEDs():
    Light LED 1
    Light LED 2
    Light LED 3

Once you’ve put them in a module like that, to light all three LEDs you just need to use the following:

LightThreeLEDs()

This makes life a lot easier, especially when you have much longer examples. My code could be much improved with further use of modules, but I didn’t have the time to go back and clean things up.

Menu Module

This module is the first thing that my ‘try’ block runs (I tell the script to run this in line 1206 later on). This starts my menu system which allows me to use the switches to select a program.

Line 361 imports the count that I set earlier in line 318. I say ‘imports’ because I use the term ‘global’. Because we start the count outside of this module, and we want to manipulate that number inside this module, we have to use ‘global’ to import the count. Something I had to learn through trial and error!

I suppose I could have just started ‘count’ in the module itself, but it works so I’m leaving it be!

Line 364 starts our while loop. I use ‘while 1’ which will run forever until I select one of the options within the loop to exit it:

def menu(): # MENU CODE FOR 4TRONIX DISPLAY BOARD AND SWITCH

    global count # IMPORTS 'COUNT' BUT LETS US EDIT IT IN A MODULE
    print "Menu start"
    
    while 1: # RUN FOREVER UNTIL WE MANUALLY EXIT THIS MODULE
    
        time.sleep(0.01)

Within my while loop are two ‘if’ statements. The while loop will run until one of these ‘if’ statements is triggered.

The first trigger (line 368) is if the Pi sees an input from GPIO ‘switch1’ which I defined earlier as clicking the switch in one direction. If this is triggered, the code then looks at my imported ‘count’ number and adds 1 to it. This means as you click the switch each time, it will always add 1 to the count.

This count change is important as it defines what shows on the IP display board. You will see in lines 372-411 each block starts with ‘if count ==’. It’s looking at the count and selecting the right block of code that matches my count.

For example, if the count is 4, it will run lines 387-390 which just sets the IP display to show the number 4.

The two lines after each ‘if’ block are changing the IP display number shown. You’ll see the N1, N2, N3 etc that I defined in the setup earlier:

if GPIO.input(switch1): # IF SWITCH 1 IS CLICKED
    time.sleep(0.8)
    count = count +1 # ADD 1 TO COUNT
    
    if count == 1: # IF 'COUNT' IS 1
        print "count is 1"
        bus.write_byte_data(addr, 0x13, 7)
        bus.write_byte_data(addr, 0x12, N1) # DISPLAY 1
        
    if count == 2: # IF 'COUNT' IS 2
        print "count is 2"
        bus.write_byte_data(addr, 0x13, 7)
        bus.write_byte_data(addr, 0x12, N2) # DISPLAY 2
        
    if count == 3: # IF 'COUNT' IS 3
        print "count is 3"
        bus.write_byte_data(addr, 0x13, 7)
        bus.write_byte_data(addr, 0x12, N3) # DISPLAY 3
        
    if count == 4: # IF 'COUNT' IS 4
        print "count is 4"
        bus.write_byte_data(addr, 0x13, 7)
        bus.write_byte_data(addr, 0x12, N4) # DISPLAY 4
        
    if count == 5: # IF 'COUNT' IS 5
        print "count is 5"
        bus.write_byte_data(addr, 0x13, 7)
        bus.write_byte_data(addr, 0x12, N5) # DISPLAY 5
        
    if count == 6: # IF 'COUNT' IS 6
        print "count is 6"
        bus.write_byte_data(addr, 0x13, 7)
        bus.write_byte_data(addr, 0x12, N6) # DISPLAY 6
        
    if count == 7: # IF 'COUNT' IS 7
        print "count is 7"
        bus.write_byte_data(addr, 0x13, 7)
        bus.write_byte_data(addr, 0x12, N7) # DISPLAY 7
        
    if count == 8: # IF 'COUNT' IS 8
        count = 1 # RETURN COUNT TO 1 TO RESET MENU
        print "...returning count to 1"
        bus.write_byte_data(addr, 0x13, 7)
        bus.write_byte_data(addr, 0x12, N1) # DISPLAY 1

The second trigger (line 414) is if the Pi sees an input from GPIO ‘switch2’. Again, I defined this earlier, and is achieved by pushing the switch in the other direction.

This section of code works in a similar way, with a number of blocks starting with ‘if count ==’, however instead of changing the display, it will run a module based on what the current count number is.

Before it jumps to the defined module, it turns off the IP display to let me know it has worked, and resets the count to 0 so that next time we come back to the menu, it will start at the beginning again:

if GPIO.input(switch2): # IF SWITCH 2 IS CLICKED
    time.sleep(0.8)
    
    if count == 1: # IF 'COUNT' IS 1
        bus.write_byte_data(addr, 0x13, 0xff)
        time.sleep(3)
        count = 0 # RESET COUNT SO MENU RETURNS TO 1 NEXT TIME
        generalcontrol() # GO TO THE GENERALCONTROL MODULE

    if count == 2: # IF 'COUNT' IS 2
        bus.write_byte_data(addr, 0x13, 0xff)
        time.sleep(3)
        count = 0 # RESET COUNT SO MENU RETURNS TO 1 NEXT TIME
        proximity() # GO TO THE PROXIMITY MODULE

    if count == 3: # IF 'COUNT' IS 3
        bus.write_byte_data(addr, 0x13, 0xff)
        time.sleep(3)
        count = 0 # RESET COUNT SO MENU RETURNS TO 1 NEXT TIME
        threepoint() # GO TO THE THREEPOINT MODULE

    if count == 4: # IF 'COUNT' IS 4
        bus.write_byte_data(addr, 0x13, 0xff)
        time.sleep(3)
        count = 0 # RESET COUNT SO MENU RETURNS TO 1 NEXT TIME
        linefollow() # GO TO THE LINEFOLLOW MODULE

    if count == 5: # IF 'COUNT' IS 5
        bus.write_byte_data(addr, 0x13, 0xff)
        time.sleep(3)
        count = 0 # RESET COUNT SO MENU RETURNS TO 1 NEXT TIME
        skittles()  # GO TO THE SKITTLES MODULE

    if count == 6: # IF 'COUNT' IS 6
        bus.write_byte_data(addr, 0x13, 0xff)
        time.sleep(3)
        count = 0 # RESET COUNT SO MENU RETURNS TO 1 NEXT TIME
        endscript() # GO TO THE ENDSCRIPT MODULE

    if count == 7: # IF 'COUNT' IS 7
        bus.write_byte_data(addr, 0x13, 0xff)
        time.sleep(3)
        count = 0 # RESET COUNT SO MENU RETURNS TO 1 NEXT TIME
        haltcommand() # GO TO THE HALTCOMMAND MODULE

General Robot Control Module

The next module I have defined is for general robot control. This is for basic remote control of the robot, and can be triggered by selecting number 1 on the IP Display menu (the part we just covered above).

The first thing we do in this module (lines 466-471) is once again import some global items. These are the speed settings for each motor, which we will want to change hence importing them this way:

# IMPORT GLOBAL VARIABLES SO WE CAN CHANGE THEM IN A MODULE
global TurnSpeed
global ForwardSpeedL
global ForwardSpeedR
global ReverseSpeedL
global ReverseSpeedR

Then I start another ‘while 1’ loop. This while loop is going to be constantly checking for button presses or depresses due to the ‘if’ statements we have inside it:

while 1: # RUN FOREVER UNTIL WE MANUALLY EXIT THIS MODULE

Next we use a ‘for’ statement to ask PyGame to keep an eye out for button presses or depresses:

for event in pygame.event.get(): # LOOK FOR A PYGAME EVENT

As PyGame is now keeping a close watch on what we press, we better tell it what we want it to do if certain keys are pressed.

In line 482 we say to PyGame “if you see a key pressed down, have a look inside this if statement for further instructions”:

if event.type == pygame.KEYDOWN: # IF A KEY IS PRESSED DOWN

Assuming we have pushed a key down, my code then defines what we want the program to do if certain keys are pressed.

First up is a commented out section (lines 485-490) that can be used to debug PyGame key presses. If you’re not getting the behaviour you expected, you can uncomment this block and it will print every key you press to the terminal. It’s great for checking that PyGame thinks it’s the same key as you do:

#======================================================
# KEY DEBUG - PRINTS THE KEY DETECTED IN THE TERMINAL (Optional)
                
#newkey = ""
#newkey = pygame.key.name(event.key)
#print newkey

Line 493-643 make up a large section of code, but it’s mostly variations of the same thing. I’ll cover the first block to explain what it does, which applies to the other sections within this range.

Let’s take lines 493-514 for example, which is the code that makes AverageBot move when pressing UP/DOWN/LEFT/RIGHT.

Line 497 is an ‘if’ statement that says “Hey PyGame, if you see the UP key pressed, do this”. If UP is pressed, this block tells the robot to move forward by referring to a function called ‘Forward’ that we define later on. The ‘Forward’ function needs a left motor and right motor speed in brackets, which we have defined in the setup as ‘ForwardSpeedL’ and ‘ForwardSpeedR’ (for the left and right motors respectively).

Similar blocks of code can be seen below this one, defining the movement in the same way for reverse, right and left movement. A full list of the available PyGame key codes can be found here.

#======================================================
# BASIC MOVEMENT (FWD, REV, RIGHT, LEFT)

# FORWARD (UP KEY)
if event.key == pygame.K_UP: # UP KEY PRESSED
    Forward(ForwardSpeedL, ForwardSpeedR) # FORWARD AT SET SPEED
    print 'Forward speed', ForwardSpeedL, ForwardSpeedR # PRINT SPEED
    
# REVERSE (DOWN KEY)
if event.key == pygame.K_DOWN: # DOWN KEY PRESSED
    Reverse(ReverseSpeedL, ReverseSpeedR) # REVERSE AT SET SPEED
    print 'Reverse speed:', ReverseSpeedL, ReverseSpeedR # PRINT SPEED

# RIGHT (RIGHT KEY)
if event.key == pygame.K_RIGHT: # RIGHT KEY PRESSED
    Right(TurnSpeed) # SPIN RIGHT AT SET SPEED
    print 'Spin right speed:', TurnSpeed # PRINT SPEED

# LEFT (LEFT KEY)
if event.key == pygame.K_LEFT: # LEFT KEY PRESSED
    Left(TurnSpeed) # SPIN LEFT AT SET SPEED
    print 'Spin left speed', TurnSpeed # PRINT SPEED

Let’s now jump to speed control within this section. I’ll use lines 517-547 as an example, but there are more variations of this in the lines following this block.

To change the speed of my robot I couldn’t just use a +10 PWM for example, as you will recall that I needed to test the robots accuracy at different speeds as it’s a bit inconsistent. This is why I set specific speed levels in the setup in lines 255-275, so that I can have three different pre-defined speed levels instead of incremental changes on the fly.

The block of code below works in the same way again – checking if a key is pressed, and then taking a specific action if it is pressed.

This code is to turn the forward and reverse speed up. The code checks what the current level of the left and right motors are , and then depending on the current level, either increses the speed to the next level or leaves it alone if it’s already at the highest level.

For example, line 525 is an ‘if’ statement that will only trigger if the left and right motors are currently set to low. If they are currently at the lowest setting, it will change the speed to medium on both motors. This is done in lines 529-532.

Similar functions work for different speed ranges in the remaining lines. If the motors are set to medium, it will switch them to high. If they’re already at the highest fast setting, it will do nothing.

You’ll notice no numbers are mentioned – this is because we set everything up earlier, allowing us to change the levels centrally rather than having to update numbers throughout the code:

#======================================================
# TURN THE FORWARD/REVERSE SPEED UP
# Looks at left AND right speed and changes both at the same time
# This maintains PWM left/right balancing to handle inaccuracies

if event.key == pygame.K_1: # 1 KEY PRESSED

    # IF SPEED IS LOW - SET TO MEDIUM
    if (ForwardSpeedL == ForwardLowL and ForwardSpeedR == ForwardLowR and ReverseSpeedL == ReverseLowL and ReverseSpeedR == ReverseLowR):
        print 'Increasing to medium speed'
        print 'Forward:', ForwardMediumL, ForwardMediumR
        print 'Reverse:', ReverseMediumL, ReverseMediumR
        ForwardSpeedL = ForwardMediumL
        ForwardSpeedR = ForwardMediumR                          
        ReverseSpeedL = ReverseMediumL
        ReverseSpeedR = ReverseMediumR
        

    # IF SPEED IS MEDIUM - SET TO HIGH
    elif (ForwardSpeedL == ForwardMediumL and ForwardSpeedR == ForwardMediumR and ReverseSpeedL == ReverseMediumL and ReverseSpeedR == ReverseMediumR):
        print 'Increasing to high speed'
        print 'Forward:', ForwardHighL, ForwardHighR
        print 'Reverse:', ReverseHighL, ReverseHighR
        ForwardSpeedL = ForwardHighL
        ForwardSpeedR = ForwardHighR
        ReverseSpeedL = ReverseHighL
        ReverseSpeedR = ReverseHighR

    # IF SPEED ALREADY HIGHEST - DO NOTHING
    elif (ForwardSpeedL == ForwardHighL and ForwardSpeedR == ForwardHighR and ReverseSpeedL == ReverseHighL and ReverseSpeedR == ReverseHighR):
        print 'Highest speed reached!'

There is also an exit option in this part of the module. Lines 622-643 handle this, which triggers when the ESCAPE key is pressed.

Lines 628-629 set the IP display back to an underscore ready for using the menu again, and lines 633-637 reset the speed levels back to default for other programs.

Finally, line 643 sends us back to the menu module to allow us to select another program:

#======================================================
# EXIT THE PROGRAM AND BACK TO MENU

if event.key == pygame.K_ESCAPE: # ESCAPE KEY PRESSED
    
    # SET SEGMENT BACK TO UNDERSCORE FOR THE MAIN MENU
    bus.write_byte_data(addr, 0x13, 7)
    bus.write_byte_data(addr, 0x12, underscore)
    time.sleep(0.5)
    
    # RESET GLOBAL SPEEDS FOR OTHER PROGRAMS THAT HAVE FIXED SPEED
    TurnSpeed = SpeedHigh
    ForwardSpeedL = ForwardHighL
    ForwardSpeedR = ForwardHighR
    ReverseSpeedL = ReverseHighL
    ReverseSpeedR = ReverseHighR

    print "Returning to main menu"
    time.sleep(0.5)
    
    # GO BACK TO MAIN MENU
    menu()

The next thing to get your head around is the concept of doing ‘something’ when a key isn’t being pressed. That seemed like an odd concept to me at first, as I would naturally expect any actions to stop when no key is being pressed.

PyGame doesn’t work that way. It needs you to tell it what to do when you stop pressing a key, otherwise it will just keep doing whatever it was doing last.

It’s easy to set up if you just want things to stop when you let go of a key. All you need to do is define an ‘if’ statement for every key that you have set up for a key press.

For example, I use the UP key to make my robot move forward, so to make the robot stop when I let go of this key, I set up a similar ‘if’ statement under the key depress ‘for’ statement.

Line 650 kicks off this section, with an ‘elif’ statement asking PyGame to keep an eye out for any keys that are depressed. Within that ‘if’ statement we do the same thing again – define what should happen for each relevant key, in the same way as before. I use ‘stop()’ here to stop the robot when each key is depressed, which is a module that I define later in the code:

elif event.type == pygame.KEYUP: # IF A KEY IS RELEASED

    # STOP ALL MOVEMENT (WHEN NO KEY PRESSED)
    
    if event.key == pygame.K_UP: # UP KEY RELEASED
        stop() # STOP MOTORS
        print 'STOP - No key press'
        
    if event.key == pygame.K_DOWN: # DOWN KEY RELEASED
        stop() # STOP MOTORS
        print 'STOP - No key press'
        
    if event.key == pygame.K_RIGHT: # RIGHT KEY RELEASED
        stop() # STOP MOTORS
        print 'STOP - No key press'
        
    if event.key == pygame.K_LEFT: # LEFT KEY RELEASED
        stop() # STOP MOTORS
        print 'STOP - No key press'
        
    if event.key == pygame.K_1: # 1 KEY RELEASED
        stop() # STOP MOTORS
        print 'STOP - No key press'
        
    if event.key == pygame.K_2: # 2 KEY RELEASED
        stop() # STOP MOTORS
        print 'STOP - No key press'
        
    if event.key == pygame.K_3: # 3 KEY RELEASED
        stop() # STOP MOTORS
        print 'STOP - No key press'
        
    if event.key == pygame.K_4: # 4 KEY RELEASED
        stop() # STOP MOTORS
        print 'STOP - No key press'
        
    if event.key == pygame.K_ESCAPE: # ESCAPE KEY RELEASED
        stop() # STOP MOTORS
        print 'STOP - No key press'

Proximity Alert Module

This module is my autonomous program for the proximity alert challenge, and is option number 2 on my display menu.

This module doesn’t need any PyGame magic as we don’t have any remote control here. The robot approaches a wall on its own, and uses the SR-04 sensor to detect when it’s within my set distances. When it gets to the set distance, it checks the distance a further 7 times.

I run the check 8 times because these SR-04 sensors can be a bit iffy at times, sometimes giving a random reading. I was finding that the sensor was stopping the robot a good 5-10cm away from the wall at times, so making it check again lets it edge closer and closer. This works well 90% of the time (it ain’t perfect!).

Let’s take a look at the first part of the code. On line 697 I start a count called ‘countme’. I use this later on to read how many times I’ve check the distance. In line 699 I start another ‘while’ loop to make sure this module keeps running until it hits a condition to stop itself. I’m using ‘while True’ this time – I don’t know why I switch from using ‘while 1’ to ‘while True’ – it all works the same!

Line 702 makes a new name called ‘dist’ and calls a module we create later on called ‘getDistance()’ to get a distance reading from the sensor:

#======================================================================
# DEFINE PROXIMITY MODULE

def proximity(): ### PROGRAM 2 ###
    
    countme = 0
    
    while True:
     
        # GET DISTANCE
        dist = getDistance()

Now that we’ve started a ‘while’ loop and got a distance reading from the sensor, we need to tell the robot what to do depending on how close it is to the wall (what the reading number is).

Let’s take lines 710-722 below as an example. The first ‘if’ statement says “if the distance is greater than or equal to (>=) 150, move forward at high speed for 1 second”. It does this by turning the motors on using the ‘Forward’ command, and then using a ‘time.sleep’ for 1 second.

After that ‘time.sleep’ second, the code cycles back round again (because it’s inside an endless while loop) and checks the distance once more. Ignore the ‘#stop()’ at the end – I previously got my robot to stop between readings but that made it a bit clunky.

The second similar looking block does the same thing, but instead determines what to do if the distance is between 51 and 150 (51 <= dist <= 150):

if dist >= 150: # IF DISTANCE EQUAL TO OR GREATER THAN 150
    #print "Distance: ", int(dist)
    #print "Distance greater than 150"
    Forward(ForwardHighL, ForwardHighR) # MOVE FORWARD FOR THE TIME IN TIME.SLEEP BELOW
    time.sleep(1)
    #stop()

elif 51 <= dist <= 150: # IF DISTANCE BETWEEN 51 AND 150
    #print "Distance: ", int(dist)
    #print "Distance 51 to 150"
    Forward(ForwardMediumL, ForwardMediumR) # MOVE FORWARD FOR THE TIME IN TIME.SLEEP BELOW
    time.sleep(0.3)
    #stop()

This carries on into smaller and smaller ranges until it hits lines 738-759. The block starting on line 738 says “if the distance is between 0 and 9, and if the ‘countme’ count is less than or equal to 7, stop the robot and add 1 to ‘countme’ – then go back into the ‘while’ loop”.

This means that when it thinks it has hit the minimum distance, I’m asking it to check 7 more times, because each time it checks and sees a distance of 0-9 it will add 1 to ‘countme’.

When ‘countme’ finally hits 8 and the robot once again sees the distance of 0-9, it will run the block of code starting on line 744. All this does is makes sure the robot has stopped, add an underscore to the display and then sends us back to the menu module:

elif 0 <= dist <= 9 and countme <= 7: # IF DISTANCE BETWEEN 0 AND 9
    print "0-8", countme
    stop() # STOP MOVING
    countme = countme+1
    time.sleep(0.5)
    
elif 0 <= dist <= 9 and countme == 8: # IF DISTANCE BETWEEN 0 AND 9
    print "countme 5..."
    stop() # STOP MOVING
    #print "PROGRAM COMPLETE - KILLING PROGRAM"
    time.sleep(0.5)
    
    # SET SEGMENT BACK TO UNDERSCORE FOR THE MAIN MENU
    bus.write_byte_data(addr, 0x13, 7)
    bus.write_byte_data(addr, 0x12, underscore)
    time.sleep(0.5)
    
    print "Returning to main menu"
    time.sleep(0.5)
    
    # RETURN TO MAIN MENU
    menu()

Three-Point Turn Module

This module is option 3 on my display menu, and runs the autonomous code for the three-point turn challenge.

There’s no magic or clever stuff used here – I simply tried to time my robot to drive and turn at the right times to achieve the movement the challenge required. It’s safe to say this didn’t work very well, so I’d recommend a smarter approach if you want to do something similar (use sensors!).

I don’t use a ‘while’ loop here as I want it to run once then stop and return to the menu. As you’ll see in the code below, I simply tell the robot to go forward for a set time, then left for a set time, then forward, reverse and so on (lines 767-786).

Once it’s done moving I tell the robot to stop (line 789), set the display to an underscore (lines 792-793) and then return to the menu (line 800). This lets me pick up the robot and start it again when I’m ready for my next try:

def threepoint(): ### PROGRAM 3 ###

    Forward(ForwardHighL, ForwardHighR) # MOVE FORWARD FOR THE TIME IN TIME.SLEEP BELOW
    time.sleep(3.5)
    
    Left(SpeedMedium) # SPIN LEFT AT SET SPEED
    time.sleep(0.6)
    
    Forward(ForwardHighL, ForwardHighR) # MOVE FORWARD FOR THE TIME IN TIME.SLEEP BELOW
    time.sleep(0.8)
    
    Reverse(ReverseHighL, ReverseHighR) # REVERSE FOR THE TIME IN TIME.SLEEP BELOW
    time.sleep(1.7)
    
    Forward(ForwardHighL, ForwardHighR) # MOVE FORWARD FOR THE TIME IN TIME.SLEEP BELOW
    time.sleep(0.8)
    
    Left(SpeedMedium) # SPIN LEFT AT SET SPEED
    time.sleep(0.5)
    
    Forward(ForwardHighL, ForwardHighR) # MOVE FORWARD FOR THE TIME IN TIME.SLEEP BELOW
    time.sleep(3.5)
    
    # STOP THE ROBOT
    stop()
    
    # SET SEGMENT BACK TO UNDERSCORE FOR THE MAIN MENU
    bus.write_byte_data(addr, 0x13, 7)
    bus.write_byte_data(addr, 0x12, underscore)
    time.sleep(0.5)
    
    print "Returning to main menu"
    time.sleep(0.5)
    
    # RETURN TO MAIN MENU
    menu()

Line Follower Module

Continuing in order of my menu, the line follower module is option 4 on my little display.

This is yet another autonomous challenge, using three line following sensors to follow a black line on a white background. I had very little confidence in the code below due to unexceptional testing results, but to my surprise it performed reasonably well on the day.

Line 814 starts a while loop for this module, and I’ve used my first ‘if’ statement on line 818 to check if the switch is pressed to exit the program. It might seem odd to start my code with the switch, but I found that if I made this the last ‘elif’ statement it rarely picked up the switch presses.

The code that checks the line sensors is in lines 827-844. I’ve used three ‘if’ statements here, one for each direction the robot is going in relation to the line – left, middle and right.

Line 826 starts an ‘if’ statement for if the robot is detecting the line on the left or middle+left sensors. When my sensors see the black line, they output a signal to the GPIO pin. What my ‘if’ statement is saying is “if I get a signal from the left sensor, or both the left and middle sensors, move the robot right to get it back to centre”.

You’ll see on line 827 I have ‘GPIO.input’ that you will have probably seen before. If the input equals 1, it means that sensor is seeing the line an is pushing power to the GPIO pin making it HIGH. I use ‘and’ to combine GPIO readings, so that I can set a condition that says “if you get a high signal from this sensor AND no signal from that sensor, do this”.

I use ‘and’ in conjunction with ‘or’ to make it do a certain thing for two combined scenarios. So for example in line 827 again, my condition should run if the left sensor is HIGH and the right is LOW –OR– the left sensor is HIGH, the middle sensor is HIGH and the right sensor is LOW.

Similar code is repeated in lines 833-844 for different conditions. I hope that all makes sense, please add a comment if not:

def linefollow(): ### PROGRAM 4 ###

## Line sensor: http://4tronix.co.uk/store/index.php?rt=product/product&product_id=144
## Logic:
##     When seeing white (off of the line) = Low logic
##     When seeing black (on the line) = High logic

    print "LineFollow script started"
    while True:
        print "while started"
        #time.sleep(0.1)
        
        if GPIO.input(switch1):
            print "linefollow script exit"
            stop()
            time.sleep(0.5)
            bus.write_byte_data(addr, 0x13, 7)
            bus.write_byte_data(addr, 0x12, underscore)
            menu()
        
        # X-0 or X-X-0
        elif (GPIO.input(lineLeft) == 1 and GPIO.input(lineRight) == 0) or (GPIO.input(lineLeft) == 1 and GPIO.input(lineMiddle) == 1 and GPIO.input(lineRight) == 0) :
            print "X-0-0"
            LastDir = 1
            Left(40) # SPIN LEFT AT LOW SPEED SETTING
            time.sleep(0.2)
            
        # 0-X or 0-X-X
        elif (GPIO.input(lineLeft) == 0 and GPIO.input(lineRight) == 1) or (GPIO.input(lineLeft) == 0 and GPIO.input(lineMiddle) == 1 and GPIO.input(lineRight) == 1):
            print "0-0-X"
            LastDir = 3
            Right(40) # SPIN RIGHT AT LOW SPEED SETTING
            time.sleep(0.2)
            
        # 0-X-0
        elif GPIO.input(lineLeft) == 0 and GPIO.input(lineMiddle) == 1 and GPIO.input(lineRight) == 0:
            print "0-X-0"
            Forward (30,30) # FORWARD AT LOW SPEED SETTING
            time.sleep(0.1)

Skittles Module

Option 5 on my display menu is the skittles module. This is a remote control program that is very similar to the general control module, but has the added function of servo control.

One thing I have realised is that I’ve duplicated a lot of code here for basic motor control that I could have put in to modules. Therefore I won’t talk about the basic controls as I’ve covered how that works already.

Let’s focus on lines 894-911 instead which are the servo control lines. I’m using PyGame again here, so that when I hit key ‘F’ on the remote it will fire the paddle. Don’t forget that earlier on in the setup I set my servos to a set position (the neutral position ready to fire).

Line 898 is an ‘if’ statement that will be true when PyGame detects the ‘F’ key being pressed. When this is true, lines 902-903 tell both of my servos to move to the positions I have set and tested. We do this by changing the duty cycle of each motor (remember to watch this video to learn more about that).

Line 906 then holds this new servo/paddle position for 1 second before moving to lines 910-911 that reset the servo/paddle position back to neutral:

#======================================================
# SERVO PADDLE CONTROL

# FIRE PADDLE
if event.key == pygame.K_f: # F KEY PRESSED
    print "Firing"
    
    # FIRE PADDLE BY MOVING SERVOS
    servo1.ChangeDutyCycle(10) #180
    servo2.ChangeDutyCycle(5) #0
    
    # WAIT 3 SECONDS
    time.sleep(1)

    # AUTOMATICALLY RESET PADDLE AFTER FIRING
    print "Resetting"
    servo1.ChangeDutyCycle(4) #0
    servo2.ChangeDutyCycle(11) #180

Script Exit Module

Option 6 on my menu is a really simple option to close down the script, should I feel the need to. I don’t think I used this at all on the day, but it was nice to have the option there if I needed it.

This module has just one line inside, which is ‘quit()’. As my code is in a ‘try’ block, running ‘quit()’ will also run the code in my ‘except/finally’ block, which closes the script and shuts down everything properly:

#======================================================================
# DEFINE CLEAN EXIT MENU OPTION (DISPLAY OFF AND GPIO CLEANUP)
                
def endscript(): ### PROGRAM 6 ###

    quit() # WILL RUN THE 'FINALLY' BLOCK AS WELL

Shut Down Module

This is option 7 on my menu, and is the last option available. Pressing my switch again will send the menu back to 1.

You guessed it – this module shuts down the Pi. This was a lot more useful than the script close option above, as it meant I could quickly shut down my robot safely after a challenge, saving as much precious battery life as possible.

I get a bit of a clash here as this module pretty much does the same stuff as the ‘except/finally’ exit block, so the Pi almost tries doing things twice at times. I haven’t had any issues with this, but it does feel a bit scrappy.

It’s another simple module of command. Line 1109 turns off the display board, line 1114 stops the motors, line 1118 cleans up the GPIO pins, line 1122 shuts down PyGame properly and finally line 1126 says goodnight and shuts down the Pi:

#======================================================================
# DEFINE HALT COMMAND (SHUT DOWN)
                
def haltcommand(): ### PROGRAM 7 ###
    print "Shutting down system"
    time.sleep(1)
    
    # TURN OFF 4TRONIX DISPLAY BOARD
    bus.write_byte_data(addr, 0x13, 0xff)
    time.sleep(0.5)
    
    # TURN OFF MOTORS
    print "MOTORS OFF"
    stop()
    time.sleep(0.5)
    
    # CLEAN UP GPIOS
    GPIO.cleanup()
    time.sleep(0.5)
    
    # QUIT PYGAME
    pygame.quit()
    time.sleep(0.5)
    
    # SHUT DOWN
    os.system("sudo halt") # After this it will try and run the finally block but will already be shutting down

That’s all of the menu modules out of the way. All we have left now is a module that defines how the motors work, another that runs the SR-04 distance sensor, and then finally the ‘except/finally’ block.

Motor Control Module

Lines 1129-1165 define the modules we use to control the robots movement via the motors. Remember earlier I used ‘Forward’, ‘Reverse’, ‘stop’ etc? They are all modules, and this is the block of code that makes them work.

Let’s take lines 1153-1158 as an example, as it all kind of works the same way. Line 1153 is the start of the ‘Forward’ module, which as the name suggests makes the robot move forward.

When we use ‘Forward’ we have to enter the left and right PWM speed in brackets after the word. I define those speeds in the setup block of code, but if you wanted to you could just enter two numbers.

The 4 lines from 1155-1158 set the PWM for the motors using the speeds we give.

Do I know exactly how this works? Not 100%, no. This is mostly because I borrowed this from my motor controller’s example script. However I understand the concept enough to allow me to sleep at night!

# FORWARD(LEFTSPEED, RIGHTSPEED): MOVES FORWARDS IN AN ARC BY SETTING DIFFERENT SPEEDS. 0 <= LEFTSPEED,RIGHTSPEED <= 100
def Forward(leftSpeed, rightSpeed):
    p.ChangeDutyCycle(leftSpeed)
    q.ChangeDutyCycle(0)
    a.ChangeDutyCycle(rightSpeed)
    b.ChangeDutyCycle(0)

SR-04 Control Module

Earlier on in the setup section we defined the GPIO pin for the SR-04 sensor. Lines 1168-1199 are full of sorcery that make the sensor work.

When I say ‘sorcery’ I really mean “I’m not sure how this works”. This is another block of code I stole from my motor controller’s example script. I know that it talks to the SR-04 sensor to get a reading, using time and maths to determine a distance number – but I can’t tell you exactly how this works. Sometimes you just have to let it beat you!

#======================================================================
# DEFINE SR-04 FUNCTIONS

# GETDISTANCE(). RETURNS THE DISTANCE IN CM TO THE NEAREST OBJECT
def getDistance():
    GPIO.setup(sonar, GPIO.OUT)
    # SEND 10US PULSE TO TRIGGER
    GPIO.output(sonar, True)
    time.sleep(0.00001)
    GPIO.output(sonar, False)
    start = time.time()
    count=time.time()
    GPIO.setup(sonar,GPIO.IN)
    
    while GPIO.input(sonar)==0 and time.time()-count<0.1:
        start = time.time()
    count=time.time()
    stop=count
    
    while GPIO.input(sonar)==1 and time.time()-count<0.1:
        stop = time.time()
        
    # CALCULATE PULSE LENGTH
    elapsed = stop-start
    
    # DISTANCE PULSE TRAVELLED IN THAT TIME IS TIME
    # MULTIPLIED BY THE SPEED OF SOUND (CM/S)
    distance = elapsed * 34000
    
    # THAT WAS THE DISTANCE THERE AND BACK SO HALVE THE VALUE
    distance = distance / 2
    return distance            

Running the Menu on Startup

It’s all well and good having everything in modules, but it’s important to remember to tell your ‘try’ block which module it should run at the start. Without this, nothing will run and your script will just close after the initial setup block.

Lines 1202-1206, right at the end of my ‘try’ block, are the only lines that aren’t in a module within the ‘try’ block. This means that technically it jumps straight to this part and says “what do you want me to do?”.

There’s only one functional line here that isn’t commented out, which simply runs ‘menu()’ which starts my menu module for the display and switches.

#======================================================================
# !! IMPORTANT !! MAKE THE SCRIPT RUN 'MENU' AT THE START !!
# Without this, nothing will happen!

menu()

Except/Finally Block

This is it, the last block of code!

This is the ‘except/finally’ block I’ve been referring to throughout this blog so far, and it’s something I learnt from a blog post by Alex Eames over at RasPi.TV.
If an error occurs, or I just decide to exit the script, this section of code will always run. This is useful to make sure no matter how the script shuts down it will always run these commands to safely kill off the various things that are running.

I have two options here – ‘finally’ or ‘except’. I think you can only use one at a time, and in this example I’ve commented out ‘finally’ and am using the ‘except’ block.

The difference I have found is that the ‘except’ option will show you the syntax error in the terminal if there’s an error, whereas using ‘finally’ seems to ignore that and shut things down properly. Seeing the error can be handy, so I switch between them at times.

The lines of code here work exactly the same as option 7 on my menu, closing down the running processes safely one at a time with a short delay in between each just to give it a chance:

#======================================================================
# FINALLY/EXCEPT BLOCK
# This will run if there is an error or we choose to exit the program
# We use Finally in the end code, but have Except ready for debuging
	
#finally:
except KeyboardInterrupt: # USE THIS OPTION FOR DEBUGGING

    print "EXIT SCRIPT"
    time.sleep(1)
    
    # TURN OFF 4TRONIX DISPLAY
    print "TURNING OFF DISPLAY"
    time.sleep(0.5)
    bus.write_byte_data(addr, 0x13, 0xff) # Set all of bank 1 to High (Off)
    
    # TURN OFF MOTORS
    print "MOTORS OFF"
    stop()
    time.sleep(0.5)
    
    # CLEAN UP GPIOS
    print "PERFORMING GPIO CLEANUP"
    time.sleep(0.5)
    GPIO.cleanup()   
    
    # EXIT PYGAME
    print "EXIT PYGAME"
    time.sleep(0.5)
    pygame.quit()
    
    # EXIT PROGRAM
    print "--- EXIT NOW ---"
    time.sleep(0.5)
    quit()

…and THAT’S IT!

Credits & Thanks

Before I close off this blog post (as if it wasn’t long enough?) and my Pi Wars Devblog as a whole, I need to thank a few people who really helped me out:

4Tronix

My main credit has to go to Gareth who owns 4Tronix.co.uk. When looking for a motor controller on the internet, Gareth was happy to have a discussion with me and answered the (many) questions I had before buying the PiRoCon controller that I used on my robot. It was a scary new world at the time, so getting some poorly documented add-on from China was not an option.

As well as support, I also made good use of the example scripts that 4tronix supply with the board, re-using many functions and blocks of code.

Twitter Friends

Another huge source of information and support has been Twitter, or more precisely, the people I follow. From random followers, to fellow competitors, to online shops and even huge distributors – I’ve had so many of my 140-character questions answered on Twitter. Thanks everyone!

Other Pi Wars Competitors

I’ve had a lot of help from fellow competitors so I need to thank those guys as well. Unlike most competitive events, the nice people in the Raspberry Pi community are more interested in helping each other than getting one up over them. What a great scene!

RS DesignSpark

I also want to say a thank you to the guys over at RS / RS DesignSpark for providing the batteries for AverageBot – they did the job perfectly with plenty of spare juice remaining. Next year’s robot will most likely include a custom PCB/add-on, which I design (including my ProtoBoards range!) with RS’s free PCB design tool –  DesignSpark PCB.

Michael Horne & Tim Richardson

Mike and Tim are the organisers behind Pi Wars. Yes, just two of them! In the same year as organising Pi Wars they also released their CamJam EduKit 3…it’s been a very busy year for them!

A huge thanks to these guys, who without which there would be no Pi Wars. Seeing them busy on the day counting scores and directing people etc shows just how selfless these chaps are. A credit to the community.

Summary

I hope that breakdown of my code was helpful. I promised it to a few people so wanted to do a thorough job of it.

If it supports your robot project, why not add a comment below or send me a picture on one of the social networks? If you’ve got a question, or I’ve written something incorrectly – don’t be shy – add a comment and let me know!

With the code written up – that’s it – I’m finished! No more Pi Wars…for this year at least. AverageBot is going into storage, but will be released to stretch his legs at Raspberry Jams and other events.

Will there be a competition next year? I hope so, but I’m not sure if the CamJam guys really want to take on that burden for another year on the trot…

Until next time…

7 Comments on "AverageBot Devblog 10 – Code Breakdown"

  1. Impressive project and posts you got there, thanks a lot for sharing ! Everything’s very clear, even for another “average” man 😀
    Your blog made me want to build my own small bot (still got to find a proper name tho); I can’t wait for my EduKit to be delivered !
    Even if you already gave all information spread all over the 10 posts, do you plan to summarize all pieces of hardware you used for AverageBot ? It would simplify a lot my shopping list ^_^

    Anyway, once again, great job, great result last weekend, and keep on writting !

    • Hi Adrien. Thanks for your comment – how about I try and break it down here? Ok here goes…

      2x Pololu 100:1 HP motors (around £13 each)
      2x RS (777-0406) 7.2V batteries (around £17 each)
      large pack of Tamiya connectors + wires (around £10)
      1x Raspberry Pi Model A (around £20)
      1x 4Tronix PiRoCon motor controller (around £17)
      1x Pololu 30T track set (around £10)
      1x Pololu micro metal gearmotor extended bracket set (around £4)
      3x Line sensors (around £2 per sensor)
      1x SR-04 sensor (around £2)
      1x SR-04 metal mount (around £5)
      2x 9g servos (around £4)
      1x Battery holder (around £1)
      2x Terminal blocks (around £6 for a pair)
      Lots of stainless allen head fixings (around £5 overall)
      Lots of stainless spacers (around £5 overall)
      Pack of thin wires (around £5)
      Laser cutting and chassis materials (call it £50 if you did it all in one hit)
      Media centre remote and USB adapter (already had this)
      Battery charger (already had this)
      Metal brackets for the PiNoon attachment (around £5)

      …and probably a lot more stuff I can’t think of!

      I don’t even want to add all that up! :/

  2. What a great work! Very instructive, thank you!

  3. How did you do at pi wars

Leave a comment

Your email address will not be published.


*


This website uses cookies. Please visit our privacy policy page for more information. Privacy policy

The cookie settings on this website are set to "allow cookies" to give you the best browsing experience possible. If you continue to use this website without changing your cookie settings or you click "Accept" below then you are consenting to this.

Close