srffr = srf08.srf08(0x72)
srffc = srf08.srf08(0x71)
srffl = srf08.srf08(0x70)
srfrr = srf08.srf08(0x73)
srfrc = srf08.srf08(0x74)
srfrl = srf08.srf08(0x75)
leftf = md03.md03(0x58)
leftr = md03.md03(0x5A)
rightf = md03.md03(0x5B)
rightr = md03.md03(0x59)
accel = 15
rotsp = 130 # speed setting for skidding
gpsp = gpsdthrd.gpspol()
gpsd = gpsdthrd.gpsd
gpsp.start()
gpscnt = 0
while gpsd.fix.longitude == 0:
if (gpscnt == 0):
if (gpscnt == 0):
gpscnt += 1
print 'waiting for gps'
print 'gps ready'
gpsh = 0
cmpsh = 0
# destination coordinates
lonend = math.radians(-3.321519)
latend = math.radians(55.913029)
def run(self):
# gps thread to get latest frames of gps data
gpsp = gpsdthrd.gpspol()
gpsd = gpsdthrd.gpsd
gpsp.start()
gpscnt = 0
# while gpsd.fix.longitude==0:
# if (gpscnt==0):
# if (gpscnt==0): gpscnt+=1
# print 'waiting for gps'
print 'gps ready'
srffr = srf08.srf08(0x72)
srffc = srf08.srf08(0x71)
srffl = srf08.srf08(0x70)
srfrr = srf08.srf08(0x73)
srfrc = srf08.srf08(0x74)
srfrl = srf08.srf08(0x75)
lon = 0
lat = 0
cmpsh = 0
HOST = '192.168.2.100'
PORT = 60000
s = socket.socket()
s.connect((HOST, PORT))
while self.running:
# GPS data
# print 'time: ',time.time()
lon = gpsd.fix.longitude
lat = gpsd.fix.latitude
if (str(lon) == 'nan'):
lon = 0.0
lat = 0.0
# print 'lat: ',lat,' lon: ',lon
s.sendall("WRITE.GPS,LAT:" + str(lat) + ",LON:" + str(lon) + ".")
# CMPS11 data
cmpsh = cmps11.heading()
# print 'cmps: ',cmpsh
s.sendall("WRITE.COMPASS," + str(cmpsh) + ".")
# SRF08 data
srfrr.doranging()
srffrm = srffr.getranging()
srffcm = srffc.getranging()
srfflm = srffl.getranging()
srfrrm = srfrr.getranging()
srfrcm = srfrc.getranging()
srfrlm = srfrl.getranging()
# print
# "FL:",srfflm,"FC:",srffcm,"FR:",srffrm,"RL:",srfrlm,"RC:",srfrcm,"RR:",srfrrm
s.sendall("WRITE.RANGEFINDERS,FL:" + str(srfflm) + ",FC:" + str(srffcm) + ",FR:" + str(srffrm) +
",RL:" + str(srfrlm) + ",RC:" + str(srfrcm) + ",RR:" + str(srfrrm) + ".")
gpsp.running = False
gpsp.join()
print 'gpsp closed'
s.close()
