def autodrive(dur):
start_time = time.time()
end_time = time.time() + dur
objWhiskers = Whiskers()
mode = FORWARD
wheels.forward(150)
while(time.time() < end_time):
time.sleep(0.1)
cdist = sonar.cdist()
ldist = sonar.ldist()
rdist= sonar.rdist()
lbump = objWhiskers.checkBumpLeft()
rbump = objWhiskers.checkBumpRight()
print ("%d %d %d" % (ldist, cdist, rdist))
print ("bumpers %d %d" % (lbump, rbump))
if (mode == FORWARD):
if (cdist < 35 or ldist <6 or rdist < 6 or lbump or rbump):
print ("turning")
wheels.stop()
if (ldist < rdist):
mode=RIGHT
wheels.backward(150, .5)
time.sleep(1)
wheels.right(150, .5)
time.sleep(1)
else:
mode=LEFT
wheels.backward(150, .5)
time.sleep(1)
wheels.left(150, .5)
time.sleep(1)
if (mode==LEFT or mode==RIGHT):
if (cdist > 50):
mode=FORWARD
wheels.forward(150)
wheels.stop()
def autodrive(dur):
start_time = time.time()
end_time = time.time() + dur
mode = FORWARD
wheels.forward(-150)
while (time.time() < end_time):
time.sleep(0.1)
cdist = sonar.cdist()
ldist = sonar.ldist()
rdist = sonar.rdist()
print("%d %d %d" % (ldist, cdist, rdist))
if (mode == FORWARD):
if (cdist < 35 or ldist < 6 or rdist < 6):
print("turning")
wheels.stop()
if (ldist < rdist):
mode = RIGHT
wheels.backward(-100)
time.sleep(1)
wheels.right(-250)
time.sleep(1)
else:
mode = LEFT
wheels.backward(-100)
time.sleep(1)
wheels.left(-250)
time.sleep(1)
if (mode == LEFT or mode == RIGHT):
if (cdist > 50):
mode = FORWARD
wheels.forward(-150)
wheels.stop()
signal.signal(signal.SIGTERM, sigterm_handler)
try:
arm.start()
while(cur_time<max_time):
time.sleep(1)
cur_time += 1
if(random.randint(1, chance_turn) == 1):
# turn
if(random.randint(1, 2) == 1 and cur_rotate_time < right_rotate_time):
add_rotate_time = min(random.randint(1, 3), right_rotate_time-cur_rotate_time)
wheels.forward(add_rotate_time)
cur_rotate_time += add_rotate_time
cur_time += add_rotate_time
else:
rem_rotate_time = min(random.randint(1, 3), cur_rotate_time-left_rotate_time)
wheels.backward(rem_rotate_time)
cur_rotate_time -= rem_rotate_time
cur_time += rem_rotate_time
arm.end()
except KeyboardInterrupt:
print('quitting')
GPIO.output(out_vdd, GPIO.LOW)
GPIO.output(out_gnd, GPIO.LOW)
GPIO.output(out_w1, GPIO.LOW)
GPIO.output(out_w2, GPIO.LOW)
GPIO.output(out_w3, GPIO.LOW)
GPIO.output(out_w4, GPIO.LOW)
GPIO.cleanup()
centerdist ''' print sys.argv if (len(sys.argv) == 1): print usage exit(1) cmd = sys.argv[1] if (cmd == 'forward'): wheels.forward(200, 1) elif (cmd == 'backward'): wheels.backward(150, 1) elif (cmd == 'left'): wheels.left(-150, 0.3) elif (cmd == 'right'): wheels.right(-150, .3) elif (cmd == 'wheel1'): wheels.spinMotor(1, 50, 1) elif (cmd == 'wheel2'): wheels.spinMotor(2, 50, 1) elif (cmd == 'wheel3'): wheels.spinMotor(3, 50, 1)
def b():
wheels.backward(200)
return ''
def backward():
wheels.backward(200, 3)
return ''
centerdist ''' print sys.argv if (len(sys.argv) == 1): print usage exit(1) cmd = sys.argv[1] if (cmd == 'forward'): wheels.forward(200, 1) elif (cmd == 'backward'): wheels.backward(200, 1) elif (cmd == 'left'): wheels.left(200, 1) elif (cmd == 'right'): wheels.right(200, 1) elif (cmd == 'wheel1'): wheels.spinMotor(1, 200, 1) elif (cmd == 'wheel2'): wheels.spinMotor(2, 200, 1) elif (cmd == 'wheel3'): wheels.spinMotor(3, 200, 1)
def autodrive(dur):
start_time = time.time()
end_time = time.time() + dur
objWhiskers = Whiskers()
timestr = time.strftime("%Y%m%d-%H%M%S")
#grab the unique id for the run from a file then increment it by 1 and store back into the file
iRunNum = 0
with open('data/current_run.txt') as f:
sRunNum=f.read()
print ("Run number is %s", str(sRunNum))
iRunNum=int(sRunNum)
iRunNum+=1
f.close()
os.remove('data/current_run.txt')
#write out the new number for the run
outf = open('data/current_run.txt', 'w')
outf.write(str(iRunNum))
outf.close()
#now build the run data directory and store its location in a flat file
sRunDataDirectory='data/'+str(iRunNum)+'-run-'+timestr+'/'
os.makedirs(sRunDataDirectory)
print('Created data Directory')
print(sRunDataDirectory)
#write the directory to a file
outf = open('run_data_directory.txt', 'w')
outf.write(str(sRunDataDirectory))
print ('Logging to %s', sRunDataDirectory)
outf.close()
#Start driving!
mode = FORWARD
wheels.forward(150)
while(time.time() < end_time):
print ("starting autonomous loop")
time.sleep(0.1)
#print ("getting cdist")
cdist = sonar.cdist()
#print ("getting ldist")
ldist = sonar.ldist()
#print ("getting rdist")
rdist= sonar.rdist()
print ("left %d centre %d right %d" % (ldist, cdist, rdist))
lbump = objWhiskers.checkBumpLeft()
rbump = objWhiskers.checkBumpRight()
print ("bumpers %d %d" % (lbump, rbump))
if (mode == FORWARD):
if (cdist < 35 or ldist <6 or rdist < 6 or lbump or rbump):
print ("turning")
wheels.stop()
if (ldist < rdist):
print ("turning RIGHT")
#mode=RIGHT
wheels.backward(150, .5)
time.sleep(1)
print ("turning RIGHT finshed reverse")
wheels.right(150, .5)
time.sleep(1)
print ("turning RIGHT done")
else:
#mode=LEFT
print ("turning LEFT")
wheels.backward(150, .5)
time.sleep(1)
print ("turning LEFT finished reverse")
wheels.left(150, .5)
time.sleep(1)
print ("turning LEFT finished")
else:
wheels.forward(150)
if (mode==LEFT or mode==RIGHT):
if (cdist > 50):
mode=FORWARD
print ("keep on cruisin' forward")
wheels.forward(150)
print ("Finished driving forward")
print ("finished autonomous driving, time to relax")
wheels.stop()