Embedded Files
Inside the Bot
#the main function
import grid as Grid
import grid_detect as gd
import motor_run
import obstacle
import time
drivers=motor_run.motors()
vision=obstacle.obstacle()
power_on=True
order_underway=False
dest_reached=False
return_to_base=False
order_table=None
route=[]
src=None
dest=None
gridsize = [4,4]
base=(1,1)
tables = [(1,2),(2,2),(3,2),(2,4),(3,4)]
exclude = []
current_location=base
next_location=None
route_prg=None
# -1 -Standstill
#0-downward(row ind increasing) , 1-upward(row ind decreasing)
#2-rightwards(col ind increasing) , 3-leftwards(col ind increasing)
req_dir=-1
heading=0
turn_req=0
taxi=0 #1 when in penultimate cell
halt=0 #1 when destination reached and food to be taken
#state of ultraonic sensors
state=-1
#grid cell data
height=10#along y-axis - col const
width=10#along x-axis - row const
#time bounds
Start_time=None
End_time=None
elapsed=None
#coordinates w.r.t top left
cur_ht=5
cur_wt=5
#time to reach midpoint
midtime=5
def order_received():
return True
def obstacle_action(req_dir,heading):#return 1 when obj successfully navigated
global vision,drivers
new_heading=heading
search_left=0
search_right=0
ret_left=0
ret_right=0
delta=0
delta_flag=0
Start_time=time.time()
End_time=None
done=0
if (req_dir==heading):
drivers.left_turn()
state=1
search_left=1
new_heading=req_dir
while (True):
line=gd.isLineThere()
obs=vision.check_for_obstacle(state)
End_time=time.time()
if (line or vision.check_for_obstacle()):
delta=End_time-Start_time
drivers.about_turn()
Start_time=time.time()
if (search_right==1):
search_right=0
ret_right=1
delta_flag=1
state=1
elif(search_left==1):
search_left=0
ret_left=1
delta_flag=1
state=2
if (not obs):
if (search_left==1 or ret_right==1):
drivers.right_turn()
return 1
elif(search_right==1 or ret_left==1):
drivers.left_turn()
return 1
if(delta_flag==1):
if (End_time-Start_time>delta):
delta=0
Start_time=time.time()
if (ret_left==1):
search_right=1
ret_left=0
else:
done=1
ret_right=0
delta_flag=0
if (done==1):
drivers.right_turn()
done=0
return 0
drivers.burst_forward()
else:
return 0
'''
if (req_dir!=heading):
state=turn_at_change(req_dir,heading)
new_heading=req_dir
while (!vision.check_for_obstacle()):
line=gd.isLineThere()
obs=vision.check_for_obstacle(state)
End_time=time.time()
if (!obs):
free_path=1
break
'''
#if (req_dir)
def turn_at_change(req_dir,cur_dir):
global drivers
if (cur_dir==0):
if (req_dir==1):
drivers.about_turn()
return -1
elif (req_dir==2):
drivers.right_turn()
return 2
elif (req_dir==3):
drivers.left_turn()
return 1
elif (cur_dir==1):
if (req_dir==0):
drivers.about_turn()
return -1
elif (req_dir==2):
drivers.left_turn()
return 1
elif (req_dir==3):
drivers.right_turn()
return 2
elif (cur_dir==2):
if (req_dir==0):
drivers.left_turn()
return 1
elif (req_dir==1):
drivers.right_turn()
return 2
elif (req_dir==3):
drivers.about_turn()
return -1
elif (cur_dir==3):
if (req_dir==0):
drivers.right_turn()
return 2
elif (req_dir==1):
drivers.left_turn()
return 1
elif (req_dir==2):
drivers.about_turn()
return -1
def decode_dir(delx,dely):
if (delx==1):
return 0
elif (delx==-1):
return 1
elif (dely==1):
return 2
elif (dely==-1):
return 3
while (power_on):
order_table=(3,4)
order_underway=True
#graph setup
src=base
dest=order_table
grid = Grid.graph()
grid.create(gridsize,tables)
grid.bfs(src)
route=grid.pathfinder(dest,exclude)
current_location=base
next_location=route[1]
route_prg=1#index of next location in route
#direction setup
req_dir=decode_dir(next_location[0]-current_location[0],next_location[1]-current_location[1])
turn_at_change(req_dir,heading)
heading=req_dir
#motor setup
drivers.setup()
#ultrasonic setup
vision.setup()
while (order_underway or return_to_base):
line=gd.isLineThere()
obst=vision.check_for_obstacle()
if (not obst):
drivers.burst_forward()
if (line):
if (route_prg==(len(route)-2)):
taxi=1
elif (route_prg==(len(route)-1)):
dest_reached=True
turn_req=0
if (route_prg<(len(route)-1)):
Start_time=time.time()
current_location=next_location
next_location=route[route_prg+1]
req_dir=decode_dir(next_location[0]-current_location[0],next_location[1]-current_location[1])
route_prg+=1
turn_req=1
for a in range(4):
drivers.burst_forward()
else:
if (turn_req==1):
End_time=time.time()
elapsed=End_time-Start_time
if (elapsed>midtime and elapsed<(midtime*1.02)):
turn_at_change(req_dir,heading)
heading=req_dir
turn_req=0
if (dest_reached and halt==0):
Start_time=time.time()
End_time=time.time()
while((not vision.check_for_obstacle()) and End_time-Start_time<(midtime)*.1):
drivers.burst_forward()
End_time=time.time()
halt=1
drivers.burst_forward()
else:
action=obstacle_action(req_dir,heading)
while(action==0):
action=obstacle_action(req_dir,heading)
time.sleep(10)
if (order_received() and dest_reached and order_underway):
order_underway=False
dest_reached=False
return_to_base=True
src=dest
dest=base
time.sleep(5)
grid.create(gridsize,tables)
grid.bfs(src)
route=grid.pathfinder(dest,exclude)
current_location=dest
next_location=route[1]
route_prg=1
req_dir=decode_dir(next_location[0]-current_location[0],next_location[1]-current_location[1])
turn_at_change(req_dir,heading)
heading=req_dir
elif (return_to_base and dest_reached):
order_underway=False
dest_reached=False
return_to_base=False
Start_time=time.time()
End_time=time.time()
while (End_time-Start_time<(midtime)*.95):
drivers.burst_forward()
End_time=time.time()
turn_at_change(0,heading)
heading=req_dir
vision.clean()
Report abuse