Our vehicle used to use this program to be driven around through a laptop installed in the car. Someone located in the vehicle would punch keys on the keyboard and would be able to accelerate/brake and turn right/left. The commands would then go through the parallel port of the laptop to a controller which would in the end command several electrical steppers motors doing the manual job. We left the GPS thread in the code so that one can see how to use threads. In effect in the real program, we also use other threads to monitor cameras and other inertial information coming from the IMU but we left them out of this code for the sake of clarity. It runs under Windows and is fast enough for the purpose at hand. The drive-by-wire code enables us to drive the vehicle around and allows the gathering of data on maneuverability and collision avoidance. The sentences said by the program through the TTS module may look like a gimmick, but they are in fact very helpful when only one person drives around and collects data at the same time.
The program starts two threads. One is watching the keyboard for instruction. The other one is recording data from the GPS sensor.
# This code is released
# under the General
# Public Licence (GPL),
# DISCLAIMER : if you do not know
# what you are doing
# do not use this program on a
# motorized vehicle or any machinery
# for that matter. Even if you know
# what you are doing, we absolutely
# cannot be held responsible for your
# use of this program under ANY circumstances.
# The program is
# released for programming education
# purposes only i.e. reading
# the code will allow students
# to understand the flow of the program.
# We do not advocate nor condone
# the use of this program in
# any robotic systems. If you are thinking
# about including this software
# in a machinery of any sort, please don't do it.
#
# This program was written by
# Pedro Davalos, Shravan Shashikant,
# Tejas Shah, Ramon Rivera and Igor Carron
#
# Light version of the Drive-by-wire program
# for Pegasus Bridge 1 - DARPA GC 2005
#
import sys, traceback, os.path, string
import thread, time, serial, parallel
import re
import struct
import msvcrt
import pyTTS
rootSaveDir = "C:\\PegasusBridgeOne\\"
gpsSaveDir = os.path.join(rootSaveDir,"gps")
ctlSaveDir = os.path.join(rootSaveDir,"control")
stopProcessingFlag = 0
printMessagesFlag = 1
p = None
tts = pyTTS.pyTTS()
gps_data = list()
imu_data = list()
def runnow():
print "Drive-by-wire program running now"
#launch controller thread
t = thread.start_new_thread(control_sensors,())
#launch gps sensor thread
t1 = thread.start_new_thread(read_gps_data,())
def control_sensors():
global stopProcessingFlag
global ctlSaveDir
global p
printMessage("Control thread has started")
p=parallel.Parallel()
p.ctrlReg = 9 #x&y axis
p.setDataStrobe(0)
sleep = .01
while (stopProcessingFlag != 1):
key = msvcrt.getch()
length = len(key)
if length != 0:
if key == " ": # check for quit event
stopProcessingFlag = 1
else:
if key == '\x00' or key == '\xe0':
key = msvcrt.getch()
print ord(key)
strsteps = 5
accelsteps=1
if key == 'H':
move_fwd(accelsteps, sleep)
if key == 'M':
move_rte(strsteps, sleep)
if key == 'K':
move_lft(strsteps, sleep)
if key == 'P':
move_bak(accelsteps, sleep)
print "shutting down"
ctl_ofile.close()
def read_gps_data():
global gpsSaveDir
global stopProcessingFlag
global gps_data
printMessage("GPS thread starting.")
#Open Serial Port with baud 4800 on port 5 (Com6)
ser = None
try:
gps_ofile = open(''.join( \
[gpsSaveDir,'\\',time.strftime**
**("%Y%m%d--%H-%M-%S-"),**
** "-gps_output.txt"]),"w")
ser = serial.Serial(5, 38400, timeout=2)
while (stopProcessingFlag != 1):
resultx = ser.readline()
# if re.search('GPRMC|GPGGA|GPZDA', resultx):
splt = re.compile(',')
ary = splt.split(resultx)
gps_ofile.write(str(get_time() + ', '+resultx))
gps_data.append([get_time(), resultx])
if (len(gps_data)> 100): gps_data.pop(0)
ser.close()
gps_ofile.close()
except:
write_errorlog("GPS Read method " )
if ser != None:
ser.close()
def write_errorlog(optional_msg=None,whichlog='master'):
if optional_msg!=None:
writelog(whichlog,"\nERROR : "+ optional_msg)
type_of_error = str(sys.exc_info()[0])
value_of_error = str(sys.exc_info()[1])
tb = traceback.extract_tb(sys.exc_info()[2])
tb_str = ''
# error printing code goes here
def get_time(format=0):
stamp = '%1.6f' % (time.clock())
if format == 0:
return time.strftime('%m%d%Y %H:%M:%S-', \
time.localtime(time.time())) +str(stamp)
elif format == 1:
return time.strftime('%m%d%Y_%H%M', \
time.localtime(time.time()))
def if_none_empty_char(arg):
if arg == None:
return ''
else:
return str(arg)
def writelog(whichlog,message):
global rootSaveDir
logfile = open(os.path.join(rootSaveDir, \
whichlog+".log"),'a')
logfile.write(message)
logfile.close()
printMessage(message)
def printMessage(message):
global printMessagesFlag
if printMessagesFlag == 1:
print message
def move_fwd(steps, sleep):
global p
for i in range (0,steps):
p.setData(32)
p.setData(0)
time.sleep(sleep)
def move_bak(steps, sleep):
global p
for i in range (0,steps):
p.setData(48)
p.setData(0)
time.sleep(sleep)
def move_lft(steps, sleep):
global p
for i in range (0,steps):
p.setData(12)
p.setData(0)
time.sleep(sleep)
def move_rte(steps, sleep):
global p
for i in range (0,steps):
p.setData(8)
p.setData(0)
time.sleep(sleep)
print 'Hello'
tts.Speak("Hello, I am Starting the control**
**Program for Pegasus Bridge One")
time.sleep(4)
runnow()
while (stopProcessingFlag != 1):
time.sleep(5)
print 'Bye now'
tts.Speak("Destination reached, Control Program**
**Aborting, Good Bye, have a nice day")
time.sleep(4)
