Chapter 11. Final Code and Conclusion

At this point in the build, you should have at least two things: a working rover, and a directory on the Pi full of small Python scripts that do all kinds of neat things separately, but don’t work together very well. As the last part of this build, we’ll have to combine all of the scripts into one large working program that does everything we expect it to.

When setting up this program,we need to ask ourselves a few questions. First, how many sensors are we going to be using? There are a lot of different ones, and you have only a limited number of GPIO pins available (unless they’re all on the I2C bus). Also, some of the sensors don’t play well together in the same program. For instance, the SHT15 temperature sensor works great when you test it by itself. But when you try to use another GPIO pin as an output (such as for a motor), the sht1x library re-declares the GPIO pin setup, which negates all the setup you do in the main script. Not to worry, of course—we can use the getTemperature() function from the BMP180 sensor to get the ambient temperature.

So once you’ve figured out what sensors you’re going to use, and where to place them, and how to wire them, all that remains is to write the final program. Simple, right? Actually, as I said at the beginning of the book, if you’ve been following along, most of this work is already done—all you’re doing is putting the pieces together.

I designed the program to be interactive. There’s no robotic autonomy in this program, as such a program would probably end up being thousands of lines of code. Rather, the program displays all pertinent sensor data (temperature, pressure, bearing, location, etc.) and then asks for input from the user as to what to do next. If you tell the rover to move forward, the program calls the moveForward() function. Then the sensors are polled, the sensor data is displayed, and the user is prompted again. The moveForward() function continues to execute until the user stops it. This allows the rover to continue to move until you tell it otherwise, rather than moving forward a few feet and then stopping and waiting for further instructions.

If you choose to build on this script and pursue autonomous behavior, your best bet is probably to go through a continuous loop, polling the sensors for data one by one. You can then put interrupts related to each sensor that tell the rover what to do if any of the sensors read a particular value.

Perhaps the most obvious use of this algorithm is to make the rover follow a preprogrammed route using GPS waypoints. By calling gpsd.fix.latitude and gpsd.fix.longitude, you can determine whether the rover should move forward, turn left, or turn right. When those values match your first set of coordinates, you can execute a preplanned action and then continue. To give you an example, using the GPS sensor only:

 # Destination: 36.21 degrees N, 116.53 degrees W.
 # When we reach that point, turn due south and
 # continue driving
 # Assume that we're approaching from the West


 while True:
     curLat = gpsd.fix.latitude
     curLon = gpsd.fix.longitude
     # Longitude degrees W are delineated with a
     # negative sign in NMEA strings
     if (curLat == 36.21) and (curLon == -116.53):
         allStop()
         spinLeft()
         time.sleep(1)
         allStop()
         takePicture()
     elif curLat < 36.21:
         # You've gone too far,
         # back up
         moveBackward()
         continue
     elif curLat > 36.21:
         # You're not there yet
         # Keep going
         moveForward()
         continue

This is by no means a complete section of code, has not been tested, and barely covers any of the possible actions based on the rover’s location, but it should give you some ideas, both of what is possible with a rover and what is required to achieve it. Programming an “intelligent” rover or robot is no small task, as you need to try to plan for all possible situations and then program responses to those situations—at least until artificial intelligence makes some serious strides forward. That, in fact, is part of the fun of robotic programming: not only trying to anticipate all possible situations, but also trying to program behaviors and algorithms so that the rover can react to those situations, as well as those that you (inevitably) didn’t think of or plan for. This makes what I call a robust program; should something unexpected arise, the rover has a default behavior that it can fall back on that will always work.

In the meantime, you’ll be driving your rover with your laptop, calling functions by pressing keys. I’ve tried to keep to the traditional gaming keymap: W to move forward, Z to move backward, A and D to move left and right, respectively, and S to stop. In addition, there are keys to raise the arm, lower the arm, and take a picture. In each case, the program asks for user input, calls the required function, waits a second, polls the sensors, clears the screen, and displays the sensor readings and the input prompt again. Before everything else starts, the program asks the user if a GPS is connected; if you don’t have one connected, the program skips the location query. This is done because if the program tries to query a nonexistent GPS module, it will break.

 # This script runs the rover,
 # displaying readings from the sensors
 # every time it gets input
 # from the user

 import time
 import os
 import RPi.GPIO as GPIO
 import subprocess
 from sht1x.Sht1x import Sht1x as SHT1x
 import smbus
 import math
 from gps import *
 import threading
 from Adafruit_BMP085 import BMP085

 GPIO.setwarnings(False)
 GPIO.setmode(GPIO.BOARD)
 GPIO.setwarnings (False)

 # Motor setup
 # 19 = IN1
 # 21 = ENA
 # 23 = IN2
 GPIO.setup(19, GPIO.OUT)
 GPIO.setup(21, GPIO.OUT)
 GPIO.setup(23, GPIO.OUT)
 # 22 = IN3
 # 24 = ENB
 # 26 = IN4
 GPIO.setup(22, GPIO.OUT)
 GPIO.setup(24, GPIO.OUT)
 GPIO.setup(26, GPIO.OUT)

 def moveForward():
     # r forward
     GPIO.output(21, 1)
     GPIO.output(19, 0)
     GPIO.output(23, 1)
     # l forward
     GPIO.output(24, 1)
     GPIO.output(22, 0)
     GPIO.output(26, 1)

 def moveBackward():
     # r backward
     GPIO.output(21, 1)
     GPIO.output(19, 1)
     GPIO.output(23, 0)
     # l backward
     GPIO.output(24, 1)
     GPIO.output(22, 1)
     GPIO.output(26, 0)

 def allStop():
     GPIO.output(21, 0)
     GPIO.output(24, 0)

 def spinRight():
     # leftforward, rightbackward
     GPIO.output(24, 1)
     GPIO.output(22, 0)
     GPIO.output(26, 1)
     GPIO.output(21, 1)
     GPIO.output(19, 1)
     GPIO.output(23, 0)

 def spinLeft():
     # rightforward, leftbackward
     GPIO.output(21, 1)
     GPIO.output(19, 0)
     GPIO.output(23, 1)
     GPIO.output(24, 1)
     GPIO.output(22, 1)
     GPIO.output(26, 0)

 # GPS setup
 gpsd = None
 class GpsPoller(threading.Thread):
     def __init__(self):
         threading.Thread.__init__(self)
         global gpsd
         gpsd = gps(mode=WATCH_ENABLE)
         self.current_value=None
         self.running = True
     def run(self):
         global gpsd
         while gpsp.running:
             gpsd.next()

 # Compass setup
 bus = smbus.SMBus(0)
 compAddress = 0x1e
 def read_byte(adr):
     return bus.read_byte_data(compAddress, adr)

 def read_word(adr):
     high = bus.read_byte_data(compAddress, adr)
     low = bus.read_byte_data(compAddress, adr+1)
     val = (high << 8) + low
     return val

 def read_word_2c(adr):
     val = read_word(adr)
     if val >= 0x8000:
         return -((65535 - val) + 1)
     else:
         return val

 def write_byte(adr, value):
     bus.write_byte_data(compAddress, adr, value)

 def getBearing():
     write_byte(0, 0b01110000)
     write_byte(1, 0b00100000)
     write_byte(2, 0b00000000)
     scale = 0.92
     x_offset = -39
     y_offset = -100
     x_out = (read_word_2c(3) - x_offset) * scale
     y_out = (read_word_2c(7) - y_offset) * scale
     bearing = math.atan2(y_out, x_out)
     if bearing < 0:
         bearing + 2 * math.pi
     return str(math.degrees(bearing))


 # Robotic arm servo setup

 def liftArm():
     for i in range(50, 90):
         subprocess.call("echo 2=" + str(i) + "/dev/servoblaster", shell=True)
         time.sleep(0.5)

 def lowerArm():
     for i in reversed(range(50, 90)):
         subprocess.call("echo 2=" + str(i) + "/dev/servoblaster", shell=True)
         time.sleep(0.5)

 # Rangefinder setup
 GPIO.setup(15, GPIO.OUT)
 GPIO.setup(13, GPIO.IN)

 def getRange():
     time.sleep(0.3)
     GPIO.output(15, 1)
     time.sleep(0.00001)
     GPIO.output(15, 0)
     while GPIO.input(13) == 0:
         signaloff = time.time()
     while GPIO.input(13) == 1:
         signalon = time.time()
     timepassed = signalon - signaloff
     distance = timepassed * 17000
     return str(distance)

 # Pressure and temperature
 bmp = BMP085(0x77)
 def getTemperature():
     return str(bmp.readTemperature())

 def getPressure():
     return str(bmp.readPressure()/1000)

 if __name__ == '__main__':
     gpsQuery = raw_input("Do you have a GPS connected? (y/n) ")
     if gpsQuery == 'y':
         gpsp = GpsPoller()
         try:
             gpsp.start()
             while True:

                 # Get command from user
                 os.system("clear")
                 print "Range to target: " + getRange()
                 print "Temp: " + getTemperature() + "C"
                 print "Pressure: " + getPressure() + "kPa"
                 print "Location: " + str(gpsd.fix.longitude)
                 + ", " + str(gpsd.fix.latitude)
                 print "Bearing: " + getBearing() + " degrees"
                 print "W = forward"
                 print "Z = backward"
                 print "A = left"
                 print "D = right"
                 print "S = stop"
                 print "O = raise arm"
                 print "P = lower arm"
                 print "I = take picture"

                 command = raw_input("Enter command(Q to quit): ")
                 if command == "w":
                     moveForward()
                     time.sleep(0.5)
                     continue
                 elif command == "z":
                     moveBackward()
                     time.sleep(0.5)
                     continue
                 elif command == "a":
                     spinLeft()
                     time.sleep(0.5)
                     continue
                 elif command == "d":
                     spinRight()
                     time.sleep(0.5)
                     continue
                 elif command == "s":
                     allStop()
                     time.sleep(0.5)
                     continue
                 elif command == "o":
                     liftArm()
                     time.sleep(0.5)
                     continue
                 elif command == "p":
                     lowerArm()
                     time.sleep(0.5)
                     continue
                 elif command == "i":
                     subprocess.call("raspistill -o image.jpg",
                     shell=True)
                     time.sleep(0.5)
                     continue
                 elif command == "q":
                     gpsp.running=False
                     gpsp.join()
                     GPIO.cleanup()
                     break
                 else:
                     print "Command not recognized. Try again."
                     time.sleep(1)
                     continue
         except (KeyboardInterrupt, SystemExit):
             gpsp.running = False
             gpsp.join()
             GPIO.cleanup()
     else:
         try:
             while True:
                 # Get command from user
                 os.system("clear")
                 print "Range to target: " + getRange()
                 print "Temp: " + getTemperature() + "C"
                 print "Pressure: " + getPressure() + "kPa"
                 print "Bearing: " + getBearing() + " degrees"
                 print "W = forward"
                 print "Z = backward"
                 print "A = left"
                 print "D = right"
                 print "S = stop"
                 print "O = raise arm"
                 print "P = lower arm"
                 print "I = take picture"

                 command = raw_input("Enter command
                 (Q to quit): ")
                 if command == "w":
                     moveForward()
                     time.sleep(0.5)
                     continue
                 elif command == "z":
                     moveBackward()
                     time.sleep(0.5)
                     continue
                 elif command == "a":
                     spinLeft()
                     time.sleep(0.5)
                     continue
                 elif command == "d":
                     spinRight()
                     time.sleep(0.5)
                     continue
                 elif command == "s":
                     allStop()
                     time.sleep(0.5)
                     continue
                 elif command == "o":
                     liftArm()
                     time.sleep(0.5)
                     continue
                 elif command == "p":
                     lowerArm()
                     time.sleep(0.5)
                     continue
                 elif command == "i":
                     subprocess.call("raspistill -o
                     image.jpg",
                     shell=True)
                     time.sleep(0.5)
                     continue
                 elif command == "q":
                     gpsp.running=False
                     gpsp.join()
                     GPIO.cleanup()
                     break
                 else:
                     print "Command not recognized.
                     Try again."
                     time.sleep(1)
                     continue
         except (KeyboardInterrupt, SystemExit):
             GPIO.cleanup()

Now, keep in mind that this is only a starter script, and it doesn’t use all of the sensors we went over in Chapter 10. Because your Pi has a limited number of GPIO pins, you may have to play around with power rails on your breadboard, adding and subtracting I2C devices, and other ways of managing the sensors on your rover. When it’s running, you’ll see a command window like that in Figure 11-1.

Figure 11-1. Rover command program window

You may even decide to design a GUI for your rover interface. If you decide to do that, I suggest researching Python’s Tkinter library. It’s functional more than fashionable, but its small learning curve makes it possible to design fully working user interfaces for your Python scripts.

Whatever you decide to do with your rover, above all have fun with it. Once you’ve solved the problems inherent in designing a rover from scratch, you can modify and tweak it to your heart’s content.

I look forward to seeing what you come up with!