#!/usr/bin/python
import gpsdthrd
import rplidar
import time
gpsp = gpsdthrd.gpspol()
gpsd = gpsdthrd.gpsd
try:
gpsp.start()
gpscnt = 0
while gpsd.fix.longitude == 0:
if (gpscnt == 0):
if (gpscnt == 0):
gpscnt += 1
print 'waiting for gps'
print 'gps ready'
print 'lat: ', gpsd.fix.latitude
print 'lon: ', gpsd.fix.longitude
print rplidar.getlines()
gpsp.running = False
gpsp.join()
except KeyboardInterrupt:
print 'KeyboardInt'
gpsp.running = False
gpsp.join()
print 'done'
y = math.sin(lonend - lon) * math.cos(latend)
x = math.cos(lat) * math.sin(latend) - \
math.sin(lat) * math.cos(latend) * math.cos(lonend - lon)
return (math.degrees(math.atan2(y, x)) + 360) % 360
# a function, which returns a distance between two polar coordinates
def poldist(th1, r1, th2, r2):
d = math.sqrt(r1 * r1 + r2 * r2 - 2 * r1 * r2 *
math.cos(th2 / 180 * math.pi - th1 / 180 * math.pi))
return d
try:
print 'activity 0'
linespol = rplidar.getlines()
gpsh = gpsheading() # heading towards the destination point
cmpsh = cmps11.heading() # compass heading
print 'linespol: ', linespol
print 'gpsh: ', gpsh
print 'cmpsh: ', cmpsh
# if farer than 1.21m from the destination point
while (gpsdist() > 1.21):
# activity 0 analyzes rplidar data and plans the path
if (activity == 0):
# state 0 checks if there is an obsticle on the line to the
# destination point
if (state == 0):
print 'state 0'
print 'lncnt: ', lncnt
# if there are no obsticles in the 7m radius
def objdet():
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
cmpsh = 0
linespol = []
firstcnt = 0
cnt = 0
objsize = 41 # defines the size(cm) of an object to look for
objtol = 3.7 # introduces tolerance parameter in cm
drivedist = 0
driveangle = 0
turnang = 0
turnangfx = 17 # defines an angle offset from the center of an object
stopdist = 37 # defines how far(cm) from an object the robot stops
try:
linespol = rplidar.getlines()
print linespol
if (len(linespol) != 0):
for lines in linespol:
# looks for objects
if (poldist(lines[0], lines[1], lines[2], lines[3]) >= objsize - objtol and
poldist(lines[0], lines[1], lines[2], lines[3]) <= objsize + objtol):
cnt += 1 # counts the number of objects
firstcnt += 1
# remembers the first object location
if (firstcnt == 1):
firstcnt += 1 # won't enter this if again
print 'turnang line: ', lines
# calculates an angle to face the center of an object
if (lines[2] - lines[0] < 0):
turnang = lines[2] - \
(lines[2] - lines[0] + 360) / 2
else:
turnang = lines[2] - (lines[2] - lines[0]) / 2
# creates an offset from the center of an object so that when the robot
# turns to that angle and gets rplidar data the first
# object will be this one
if (turnang - turnangfx < 0):
turnang = turnang - turnangfx + 360
else:
turnang = turnang - turnangfx
# check all the objects found
for x in xrange(0, cnt):
print 'turnang: ', turnang
cmpsh = cmps11.heading()
# the robot turns until the heading is turnang
if (turnang - cmpsh > 180):
cmpsh += 360
elif (turnang - cmpsh < -180):
cmpsh -= 360
while (turnang - cmpsh < -1.5 or turnang - cmpsh > 1.5):
cmpsh = cmps11.heading()
print 'cmpsh: ', cmpsh
if (turnang - cmpsh > 180):
cmpsh += 360
elif (turnang - cmpsh < -180):
cmpsh -= 360
# turn to the left
if (turnang - cmpsh < 0):
leftf.move(-rotsp, accel)
leftr.move(-rotsp, accel)
rightf.move(rotsp, accel)
rightr.move(rotsp, accel)
# turn to the right
elif (turnang - cmpsh > 0):
leftf.move(rotsp, accel)
leftr.move(rotsp, accel)
rightf.move(-rotsp, accel)
rightr.move(-rotsp, accel)
time.sleep(0.11)
leftf.move(0, accel)
leftr.move(0, accel)
rightf.move(0, accel)
rightr.move(0, accel)
time.sleep(1)
firstcnt = 0
scndcnt = 0
linespol = rplidar.getlines()
print linespol
for lines in linespol:
print 'firstcnt: ', firstcnt
print 'scndcnt: ', scndcnt
if (scndcnt == 2):
break
# looks for objects
if (poldist(lines[0], lines[1], lines[2], lines[3]) >= objsize - objtol and
poldist(lines[0], lines[1], lines[2], lines[3]) <= objsize + objtol):
firstcnt += 1
scndcnt += 1
if (firstcnt == 1):
print 'firstcnt line: ', lines
firstcnt += 1
# calculates the time to drive back after facing an object in order to
# get back to the starting position
drivedist = lines[1] - stopdist
# calculates an angle to face the center of an
# object
if (lines[2] - lines[0] < 0):
driveangle = lines[
2] - (lines[2] - lines[0] + 360) / 2
else:
driveangle = lines[2] - \
(lines[2] - lines[0]) / 2
if (scndcnt == 2):
print 'scndcnt line: ', lines
# calculates an angle to face the center of an
# object
if (lines[2] - lines[0] < 0):
turnang = lines[2] - \
(lines[2] - lines[0] + 360) / 2
else:
turnang = lines[2] - (lines[2] - lines[0]) / 2
# creates an offset from the center of an object so that when the robot
# turns to that angle and gets rplidar data the
# first object will be this one
if (turnang - turnangfx < 0):
turnang = turnang - turnangfx + 360
else:
turnang = turnang - turnangfx
print 'driveangle: ', driveangle
print 'drivedist: ', drivedist
cmpsh = cmps11.heading()
if (driveangle - cmpsh > 180):
cmpsh += 360
elif (driveangle - cmpsh < -180):
cmpsh -= 360
# turns to face the center of an object
while (driveangle - cmpsh < -1.5 or driveangle - cmpsh > 1.5):
cmpsh = cmps11.heading()
print 'cmpsh: ', cmpsh
if (driveangle - cmpsh > 180):
cmpsh += 360
elif (driveangle - cmpsh < -180):
cmpsh -= 360
# turn to the left
if (driveangle - cmpsh < 0):
leftf.move(-rotsp, accel)
leftr.move(-rotsp, accel)
rightf.move(rotsp, accel)
rightr.move(rotsp, accel)
# turn to the right
elif (driveangle - cmpsh > 0):
leftf.move(rotsp, accel)
leftr.move(rotsp, accel)
rightf.move(-rotsp, accel)
rightr.move(-rotsp, accel)
time.sleep(0.07)
leftf.move(0, accel)
leftr.move(0, accel)
rightf.move(0, accel)
rightr.move(0, accel)
time.sleep(0.5)
srfrr.doranging()
#srffrm = srffr.getranging()
srffcm = srffc.getranging()
# drives forward until the robot faces the object adjusting its
# path on the way
while(srffcm > stopdist):
cmpsh = cmps11.heading()
print 'cmpsh: ', cmpsh
if (driveangle - cmpsh > 180):
cmpsh += 360
elif (driveangle - cmpsh < -180):
cmpsh -= 360
if (driveangle - cmpsh > -1.5 and driveangle - cmpsh < 1.5):
leftf.move(250, accel)
leftr.move(250, accel)
rightf.move(250, accel)
rightr.move(250, accel)
elif (driveangle - cmpsh < 0):
leftf.move(170, accel)
leftr.move(170, accel)
rightf.move(250, accel)
rightr.move(250, accel)
elif (driveangle - cmpsh > 0):
leftf.move(250, accel)
leftr.move(250, accel)
rightf.move(170, accel)
rightr.move(170, accel)
srfrr.doranging()
#srffrm = srffr.getranging()
srffcm = srffc.getranging()
# print 'front: ',srffcm
#srfflm = srffl.getranging()
time.sleep(0.11)
leftf.move(0, accel)
leftr.move(0, accel)
rightf.move(0, accel)
rightr.move(0, accel)
time.sleep(2)
print 'image processing'
# add code for image processing
# drives back to the initial location
start = time.time()
while(time.time() - start < drivedist / 52):
leftf.move(-255, accel)
leftr.move(-255, accel)
rightf.move(-255, accel)
rightr.move(-255, accel)
time.sleep(0.1)
leftf.move(0, accel)
leftr.move(0, accel)
rightf.move(0, accel)
rightr.move(0, accel)
if (cnt == 0):
print 'No objects detected'
else:
print 'Done'
except KeyboardInterrupt:
leftf.move(0, accel)
leftr.move(0, accel)
rightf.move(0, accel)
rightr.move(0, accel)
print 'KeyboardInterrupt in object detection'
