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'
#!/usr/bin/python import time import md03 leftf = md03.md03(0x58) leftr = md03.md03(0x5A) rightf = md03.md03(0x5B) rightr = md03.md03(0x59) accel = 15 start = time.time() while time.time() - start < 1 / .52: leftf.move(-255, accel) leftr.move(-255, accel) rightf.move(-255, accel) rightr.move(-255, accel) leftf.move(0, accel) leftr.move(0, accel) rightf.move(0, accel) rightr.move(0, accel) print 'done'
def manualmode(): print 'enetred manual_mode' # sensors gps = "GPS." cmps = "COMPASS." srf08 = "RANGEFINDERS." lidar = "LIDAR." # Tiberius status tiberius_status = "TIBERIUS_STATUS." autonomy = "AUTONOMY_MODE" idle = "IDLE_MODE" manual = "MANUAL_MODE" leftf = md03.md03(0x58) leftr = md03.md03(0x5A) rightf = md03.md03(0x5B) rightr = md03.md03(0x59) accel = 0 right = 0 left = 0 srf = [] data = 0 HOST = '' # all available interfaces PORT = 58000 # arbitrary port # create a new socket and bind the socket to the PORT s = socket.socket(socket.AF_INET, socket.SOCK_STREAM) s.bind((HOST, PORT)) # listen only for up to 1 socket clients. s.listen(1) connection, address = s.accept() # accept a client connection. print "Connected to address = {0}".format(address) while (dtbcoms.getdata(tiberius_status) == manual): print 'entered manual mode' try: # print 'BEFORE DATA = {0}'.format(data) data = connection.recv(1024) # print 'AFTER DATA = {0}'.format(data) # left speed data format received: Left,speed value. # right speed data format received: Right,speed value. # string matching is necessary to remove excessive values from buffers. # only last value received is used. if (data.find("Left") > -1): # STRING MATCHING to find the spped value for the left side left = re.sub( r'(\w+,-?\d+.)*Left,(-?\d+).(\w+,-?\d+.)*$', r'\2', data) # print "LEFT = {0}".format(left) if (data.find("Right") > -1): # STRING MATCHING to find the speed value for the right side right = re.sub( r'(\w+,-?\d+.)*Right,(-?\d+).(\w+,-?\d+.)*$', r'\2', data) # print "RIGHT = {0}".format(right) srf = dtbcoms.getdata(srf08) # check if there is an obsticle in front if ((srf[0] < 37) or (srf[1] < 37) or (srf[2] < 37)): # check if robot is trying to drive forward if ((int(left) < 0) and (int(right) < 0)): leftf.move(int(left), accel) leftr.move(int(left), accel) rightf.move(int(right), accel) rightr.move(int(right), accel) else: print'Obsticle in front' leftf.move(0, accel) leftr.move(0, accel) rightf.move(0, accel) rightr.move(0, accel) # check if there is an obsticle behind elif ((srf[3] < 27) or (srf[5] < 27)): # check if robot is trying to drive back if ((int(left) >= 0) and (int(right) >= 0)): leftf.move(int(left), accel) leftr.move(int(left), accel) rightf.move(int(right), accel) rightr.move(int(right), accel) else: print'Obsticle behind' leftf.move(0, accel) leftr.move(0, accel) rightf.move(0, accel) rightr.move(0, accel) else: leftf.move(int(left), accel) leftr.move(int(left), accel) rightf.move(int(right), accel) rightr.move(int(right), accel) if not data: break except KeyboardInterrupt: break leftf.move(0, accel) leftr.move(0, accel) rightf.move(0, accel) rightr.move(0, accel) connection.close() # stop the connection if no more data is sent. print 'manual mode connection.close'