# Autonomous Vehicle

I am coaching Caleb, Ankit, and Matthias for the Washington State Science and Engineering Fair this year.  In the end, we ultimately want to build a vehicle that can get from Washington to Hawaii.  Most likely a sailboat…

However, for the science and engineering fair, we are testing out the navigation mechanism.  The idea is to retrofit an RC car into an autonomous vehicle that can drive itself to a specific latitude and longitude.  Testing a car is much easier than the logistics of testing a water vehicle.

The basic idea is to use a GPS to figure out the current location of the car.  Then we calculate the desired direction toward the designated waypoint.  Then, we use a compass to figure out the current heading, then calculate if the car needs to turn left, right, or go straight.  A Raspberry Pi makes all these calculations, then outputs to a set of relays that send power to the appropriate motors on the RC car.

I thought I would share in blog form.  But, if you want the super-technical details – Caleb, Ankit, and Matthias can share their Project Notebook with you.

Here is the list of major components:

We also used a lot of these connectors.

This is how the RC Car looked like when it was shipped to us.  We then took it apart to expose the RC controller and wiring.

## The Car

We figured out which wires went to the steering, and which ones went to the motors to propel it forward.  For the steering, there were 2 wires.  To turn left, you sent 7.5V into one wire and grounded the other.  To turn right, you reverse the polarity.  To make it go forward, there were two wires where one is ground, and the other has 7.5V going into it.  This is not a car that has variable speeds or variable turning.  It is all just on/off binary switches.

## The Compass

The compass is an Acrobotic QMC5883L Triple-Axis Compass.  Apparently, there is a popular compass called HMC5883L, but the one we got required a special library.  We found a python library for it here.   The wiring was the same as for the HMC5883L and we used this guide.  The compass module uses I2C which you can learn about here.  You have to enable I2C protocol using raspi-config to use I2C.  We connected the following:

• Compass VCC –> Rpi 2 (5V)
• Compass Gnd –> Rpi 6 (ground)
• Compass SCL –> Rpi 5 (SCL [Clock Line])
• Compass SDA –> Rpi3 (SDL [Data Line])

We got it to start returning data using this code.

The problem was that as we turned the compass, get_bearing() was getting all kinds of strange numbers.  What we expected was that it would return 0 degrees when pointed North, then increment to 360 degrees when we turned it clockwise.  What we found was that as we turned the compass, the return value fluctuated between 20 degrees and 200 degrees with no rhyme or reason.

We tested the compass by holding a magnet on different sides of the compass.  When we did that, it appeared to work.  We though that it could be interference from the raspberry pi, battery, and the car motors.  We decided to mount the magnet as far away from the car as possible.  Matthias integrated a post that would be built into the chassis.  Ankit created long lead wires so that we could mount the compass about 20 inches above the car using a balsa wood stick.  We tried to solder wires directly to the compass because it seemed like the compass was blipping out when we chained a bunch of wires together.  But, it was too difficult to solder the wires securely to the leads.  We ended up soldering together the male/female wires to long wires and plugging it back in to the compass and raspberry pi.

We were somewhat concerned that the car would tip while turning so we used the lightest weight material that was practical.  While Matthias was printing the new chassis, Caleb did more testing away from all electronics, and still found that the compass was returning whacky numbers.  Ended the day in discouragement.

Next day, with a new resolve to solve the issue, we ordered a new compass.  Then, we did some more reading.  We went back to where we found the python library and found the answer.  The problem is that this is a 3-axis magnetometer.  Which means that when we ask it to return a bearing, it has no idea which orientation we are using for the compass.  It ends up that we had to use a calibration program and turn the compass after we have mounted it in the orientation we were going to use it in.  Once we calibrated it, the compass worked like a champ.  I guess now we just have an extra compass.

We also didn’t need a super tall tower to mount the compass on.  We shortened the stick to about 6 inches and mounted the compass on top of the stick.

## The GPS

First, we tried to use an old USB GPS receiver made by Pharos USA.  We were not able to get it to work with the Raspberry Pi.  It’s unclear if it was broken, or if it just does not work with Raspberry Pi.  It seems like someone was able to make it work.  We could not.  So, we ordered a new USB GPS dongle.

To install the GPS, we watched this video:

First we want to see which serial port the GPS is plugged into.  Before plugging in the USB GPS, open a terminal, then type:

cd /dev

ls

Then, we look at the list of the tty section.  Plug in the GPS, then type:

ls

See if there is a new entry in the tty section.  That is your port. Ours was tty
ACM0

To read the GPS data, we use a package called gpsd.  In shell, we type in:

sudo apt-get install gpsd gpsd-clients python-gps

sudo systemctl stop gpsd.socket

sudo systemctl disable gpsd.socket

Then, we need to edit a config file:

sudo nano /lib/systemd/system/gpsd.socket

We need to change the

ListenStream=127.0.0.0:2947

to

ListenStream=0.0.0.0:2947

Then save the file.

Then, we are going to kill all gpsd processes.

sudo killall gpsd

Then, we want to tell the gpsd application where to get the data from.

sudo gpsd /dev/ttyACM0 –F /var/run/gpsd.sock

Now we are ready to see if we can find gps data by typing in gpsmon

We created a python file with the following:

```from gps import *
import time

gpsd = gps(mode=WATCH_ENABLE|WATCH_NEWSTYLE)
try:
while True:
report = gpsd.next() #
if report['class'] == 'TPV':
print("TPV")
print(getattr(report,'lat',0.0),"\t")
print(getattr(report,'lon',0.0),"\t")

except (KeyboardInterrupt, SystemExit): #when you press ctrl+c
print("Done.\nExiting.")```

One of the issues with GPS, is that it takes about a second to get a reading.   This article helped us understand the issue better.  We were having problems because the GPS was taking a second to read the GPS data, and the car would pause during that second until it got a reading.   The issue will be discussed more in the programming section and how we addressed it.

## The Relay

The relay was one of the first problems that we tackled.  We were considering using transistors to amplify the current to the motors, but ultimately decided that relays were going to be simpler for us to use.  We purchased this one from Amazon.

We watched this video to figure out how to control the relay module with the raspberry pi.

One of the issues with using a relay like this on a Raspberry Pi, is that the Raspberry Pi GPIO pins are 3.3V pins.  In order to use 3.3V pins, we had to connect a 5V line out of the raspberry Pi to the relay.

We used the following code to test the relay:

```import RPi.GPIO as GPIO
import time
GPIO.setmode(GPIO.BOARD)
GPIO.setup(32, GPIO.OUT)
GPIO.setup(36, GPIO.OUT)

for x in range(20):
print("LEFT")
GPIO.output(32, GPIO.HIGH)
GPIO.output(36, GPIO.LOW)
time.sleep(1)
print("STRAIGHT")
GPIO.output(32, GPIO.HIGH)
GPIO.output(36, GPIO.HIGH)
time.sleep(1)
print("RIGHT")
GPIO.output(32, GPIO.LOW)
GPIO.output(36, GPIO.HIGH)
time.sleep(1)
GPIO.cleanup()```

It’s worth noting that pushing the GPIO Pin to Low will activate the relay.  High will turn off the relay.

When the car is supposed to turn left, the IN3 LED is on, and IN2 LED is off.  This puts 7V to the steering motor and turns the steering wheel.

When turning right, IN3 LED is off, and IN2 is on.  This reverses the polarity to the steering motor and it turns the other direction.

When going straight, both IN2 and IN3 were off.  Both leads to the steering motor is grounded and the truck drives straight.

The Raspberry Pi pin 38 drives the motor that moves the truck forward.

In one of the field tests, we saw that some of the relays stopped working.  We thought that it was broken and we needed to purchase a new relay.  However after some reading, it ends up that the relays require a certain amount of power to energize the relays.  When we got home, we noticed that when we logged into the raspberry pi, there was an indicator that the raspberry pi was connected to a battery source without adequate power.  When we charged the battery back up, this indicator went away and the relay started working again.  As we were figuring this out, we tried multiple batteries, and multiple USB cords.  Some batteries did not provide adequate power even at full charge.  The recommended power rating for a power supply is 2A.  We also noted that some USB cables did not provide adequate power to the raspberry pi.

## The Chassis

The chassis was 3D printed by Matthias.  He used OpenSCad. The first print was quite good, but there were some parts that were slightly different than the contour of the RC Truck case and it did not sit flush.  When Caleb asked him to print a chassis that can hold a balsa wood stick (see Compass section), he made some tweaks to the design so that the chassis sits perfectly on the RC Truck casing.  The chassis is printed out using ABS plastic.  It was fastened to the chassis using sticker velcro, then reinforced with electrical tape (which also held down the relays.

## The Programming

Here is the final program:

```import math
import py_qmc5883l
from time import sleep
from gps import *
import time
import RPi.GPIO as GPIO

GPIO.setmode(GPIO.BOARD)
GPIO.setup(32, GPIO.OUT)
GPIO.setup(36, GPIO.OUT)
GPIO.setup(38, GPIO.OUT)

#Tahlequah Vashon Island
#long1 = -122.516422
#lat1 = 47.355398

#sac field
#long1 = -122.314953
#lat1 = 47.335799

#sac Parking Log
#long1 = -122.317131
#lat1 = 47.334774

#TJ corner field by red building
#long1 = -122.279001
#lat1 = 47.345857

#sac baseball field homeplate
long1 =-122.314747
lat1 = 47.335092

tolerancelat = .0001
tolerancelong = .00003

gpsd = gps(mode=WATCH_ENABLE|WATCH_NEWSTYLE)

sensor = py_qmc5883l.QMC5883L()
sensor.calibration = [[1.00579223e+00, -9.69727879e-03, -8.50240184e+02],  [-9.69727879e-03, 1.01623506e+00, -1.77743179e+03],  [0.00000000e+00, 0.00000000e+00, 1.00000000e+00]]
sensor.declination = 15.5

used = False

f = open("bearings.dat", "w")
g = open("coordinates.dat", "w")
def calculate_correct_bearing(currentbearing):
newbearing = currentbearing+50
if (type(currentbearing) != float):
raise TypeError("Only floats are supported as arguments")
else:

newbearing = 360-currentbearing-45

if newbearing >= 360:
newbearing = newbearing-360

elif newbearing < 0:
newbearing += 360
return newbearing

def goLeft():
print("LEFT")
GPIO.output(32, GPIO.HIGH)
GPIO.output(36, GPIO.LOW)
def goRight():
print("RIGHT")
GPIO.output(32, GPIO.LOW)
GPIO.output(36, GPIO.HIGH)
def goStraight():
print("STRAIGHT")
GPIO.output(32, GPIO.HIGH)
GPIO.output(36, GPIO.HIGH)
def goForwards():
GPIO.output(38, GPIO.LOW)
print("forwards")
def stop():
GPIO.output(38, GPIO.HIGH)

def calculate_initial_compass_bearing(pointA, pointB):
"""
Calculates the bearing between two points.
The formulae used is the following:
θ = atan2(sin(Δlong).cos(lat2),
cos(lat1).sin(lat2) − sin(lat1).cos(lat2).cos(Δlong))
:Parameters:
- `pointA: The tuple representing the latitude/longitude for the
first point. Latitude and longitude must be in decimal degrees
- `pointB: The tuple representing the latitude/longitude for the
second point. Latitude and longitude must be in decimal degrees
:Returns:
The bearing in degrees
:Returns Type:
float
"""
if (type(pointA) != tuple) or (type(pointB) != tuple):
raise TypeError("Only tuples are supported as arguments")

x = math.sin(diffLong) * math.cos(lat2)
y = math.cos(lat1) * math.sin(lat2) - (math.sin(lat1)
* math.cos(lat2) * math.cos(diffLong))

initial_bearing = math.atan2(x, y)

# Now we have the initial bearing but math.atan2 return values
# from -180° to + 180° which is not what we want for a compass bearing
# The solution is to normalize the initial bearing as shown below
initial_bearing = math.degrees(initial_bearing)
compass_bearing = (initial_bearing + 360) % 360

return compass_bearing

def atTarget(pointA, pointB):
if abs(pointA[0]-pointB[0]) < tolerancelat and abs(pointA[1]-pointB[1]) < tolerancelong:
return True
else:
return False

def steering():
global used, cur_long, cur_lat
if used!=True:
used = True
report = gpsd.next()
print("after gpsd")
if report['class'] == 'TPV':
print("TPV\n")
cur_long = float(getattr(report,'lon',0.0))
cur_lat = float(getattr(report,'lat',0.0))

try:
except:
print("problem with bearing")

if necessarychange <= -180:
necessarychange += 360

if necessarychange >= 180:
necessarychange -= 360

f.write(str(round(starttime - time.time(), 3)))
f.write(" seconds into testing.")
f.write("\n")
g.write(str(cur_lat))
g.write(", ")
g.write(str(cur_long))
g.write("\n")
if necessarychange >= 10:
goLeft()
f.write("We need to turn left. We need to be going at " + str(projectedHeading) + ", our current heading is " + str(heading) + ", and therefore the change we need to make is more than 10 degrees, or " + str(necessarychange)+"\n")
elif necessarychange <= -10:
goRight()
f.write("We need to turn right. We need to be going at " + str(projectedHeading) + ", our current heading is " + str(heading) + ", and therefore the change we need to make is more than 10 degrees, or " + str(necessarychange)+"\n")
else:
goStraight()
f.write("We should be going straight. We need to be going at " + str(projectedHeading) + ", our current heading is " + str(heading) + ", and therefore the change we need to make is less than 10 degrees, or " + str(necessarychange)+"\n")
time.sleep(.3)
goStraight()
else:
print("NO TPV\n")
goStraight()

used = False

m = sensor.get_magnet()
report = gpsd.next()
while report['class'] != 'TPV':
report = gpsd.next()
if report['class'] == 'TPV':
cur_long = float(getattr(report,'lon',0.0))
cur_lat = float(getattr(report,'lat',0.0))
starttime = time.time()
m = sensor.get_magnet()
while atTarget((lat1,long1), (cur_lat, cur_long)) == False:
#try:
#except:
#print("error magnet")
#continue

x.start()

goForwards()
time.sleep(.1)
stop()
time.sleep(.1)

stop()
print("we're here!")
f.close()
g.close()```

There are a ton of changes made from our first iteration.

A few things we learned along the way:

1. We found a function that calculates a heading when you give 2 GPS coordinates (lat and long).  It ends up that the heading calculated counted upperwards in a counter-clockwise direction.  The compass heading counted upward in the clockwide direction.  0 degrees and 180 degrees worked great, but 90 and 270 were total opposites.  It took a while to figure out why the car was not turning in the right direction.
2. When determing whether to turn left or right, we had to process the necessary change direction so that it outputs -180 to 180.  Initially, it would turn right if the necessary change was between 0 and 270.  It would turn left, if the necessary change was between -90 and 0.  It would eventually get to the target, but sometimes it took the long way of turning around.
3. The biggest problem we found was that the when we applied the battery power directly to the go forward motors, it started smoking after about 5 seconds.  We tried adding resistors to lower the voltage through the motors, but then the motors did not work.  In the end, we decided to pulse the motors, turning it on and off every .1 seconds.  The problem with the original code was that we were doing the steering calculation in synchronously, and it would take about a second to get a GPS reading to calculate the steering direction.  Initially, it would turn the motor on for 1 second at a time, and tend to over correct on the steering.  The car behaved like it was a drunken driver.  It took us a while to figure out that the GPS reading was the issue.  Once we figured it out, we learned about asynchronous techniques in Python and put the entire steering functionality into a thread.  If the GPS is being read, we created a variable called “used” so that it is not processed while the GPS was busy.
4. We noticed that sometimes, the program would crash leaving the relays in the ON position, then the truck would run away from us in a random direction.  We figured out that sometimes, the compass was not returning a valid value and it was making the program crash.  We put a try/catch around the compass data reading functionality to handle invalid values.
5. We started logging heading/GPS location data into a file.  Then, we separated into two files, so we can analyze the data easier.
6. We noticed in our initial runs, that the truck would stop 20-30 feet way from the target.  We experimented with the latitude and longitude tolerance to dial it in.  If the tolerance is too small, the truck would never find the location, and it would just circle around near the target.  If the tolerance is too large, then it would stop too far away from the target. location.   Latitude and Longitude tolerance had to be independent because 1 degree of latitude and 1 degree of longitude are very different depending on what latitude you are on.

Here are the videos:

Trying to send the car to home base at the Sacajawea baseball field.  Car attacks an unsuspecting Ankit.

Stops way before the target.  Need to tighten up the lat/long tolerance.

Program crashes.  Caleb and Ankit run around trying to catch the car.  Figured out that the crash was caused by invalid data from the compass.

A pretty nice run… but driving like a drunken sailor – inconsistent pulsing, over-steering.  And it can probably get closer to the target.

Finally, it works!!  But, still drives a bit like a drunken driver.  Need to see if we can make the pulsing more consistent and quicker.

Trying to send the car to the middle of the Sacajawea Soccer field. Implemented the asynchronous steering, so the pulsing is nice and consistent.  Over steering problem was also corrected.  But lat/long tolerance was too tight.  It kept circling around the target and didn’t stop.

Finally works perfect!  Stopped right on the money!

A few next steps:

1. can we do collision avoidance using a camera and OpenCV?

2. Does anyone have a drone that can take an aerial video so that we can plot the logged GPS against the actual path?