def scan_qr():
ideal_qr_us_dist = 19.5
last_error = 0
timer = 0.1
#Thread( target=qr_scanner.scan)
sleep(1.5)
if check_function():
return True
while True:
error = ideal_qr_us_dist - us.dist(qr_trig, qr_echo)
diff_error = (error - last_error) / feedback_loop.delay
last_error = error
Kp = 0.5
Kd = 0.25
P = Kp * error
D = Kd * diff_error
pid_value = P + D
if check_function:
return True
if us.dist(us.qr_trig, us.qr_echo) < ideal_qr_us_dist:
motor.set_speed('b', -40 - pid_value)
sleep(0.25)
motor.set_speed('b', 0)
if check_function():
return True
else:
motor.set_speed('r', -50)
sleep(timer)
motor.set_speed('r', 50)
sleep(0.5)
if check_function():
return True
motor.set_speed('l', -50)
sleep(timer)
sleep(0.5)
if check_function():
return True
motor.sleep('l', 50)
sleep(timer)
sleep(0.5)
if check_function():
return True
motor.set_speed('b', 0)
timer += 0.05
motor.set_speed('b', 0)
return True
def forward():
integral_error = 0
last_error = feedback_loop.ideal_right_wall_dist - us.dist()
while go_forward == True:
values = feedback_loop.adjustment_values(us.dist(), integral_error,
last_error)
pid_value = values[0] / 100
integral_error = values[1]
last_error = values[2]
print(pid_value)
motor.set_speed('right', motor.default_motor_speed - pid_value)
motor.set_speed('left', motor.default_motor_speed + pid_value)
time.sleep(0.150)
def check(direction):
return_value = True
if direction == 'right':
if us.dist(us.right_trig, us.right_echo) < 15:
return_value = False
elif direction == 'front':
if us.dist(us.front_trig, us.front_echo) < 15:
return_value = False
elif direction == 'left':
if us.dist(us.left_trig, us.left_echo) < 20:
return_value = False
return return_value
def scan_qr():
ideal_qr_us_dist = 19.5
timer = 0.25
#Thread( target=qr_scanner.scan)
sleep(1.5)
if check_function():
return True
while True:
error = ideal_qr_us_dist - us.dist(qr_trig, qr_echo)
Kp = 2
P = Kp * error
if check_function:
return True
if us.dist(us.qr_trig, us.qr_echo) < ideal_qr_us_dist:
motor.set_speed('b', -50 - P)
sleep(0.25)
motor.set_speed('b', 0)
if check_function():
return True
else:
motor.set_speed('r', -50)
sleep(timer)
motor.set_speed('r', 50)
sleep(0.5)
if check_function():
return True
motor.set_speed('l', -50)
sleep(timer)
sleep(0.5)
if check_function():
return True
motor.sleep('l', 50)
sleep(timer)
sleep(0.5)
if check_function():
return True
motor.set_speed('b', 0)
timer += 0.05
motor.set_speed('b', 0)
return True
def wait_for_path():
while us.dist(us.front_trig, us.front_echo) < 14 or check_for_qr():
time.sleep(0.5)
pass
print('block removed')
thread_forward()
#so that it goes to the next cell
time.sleep(1)
def forward(direction):
if direction == 'right':
trigger = us.right_trig
echo = us.right_echo
other = 'left'
if direction == 'left':
trigger = us.left_trig
echo = us.left_echo
other = 'right'
speed = motor.default_motor_speed + 30
global go_forward
integral_error = 0
go_forward = True
last_error = feedback_loop.ideal_right_wall_dist - us.dist(trigger, echo)
while go_forward == True:
values = feedback_loop.adjustment_values(direction,
us.dist(trigger, echo),
integral_error, last_error)
pid_value = values[0]
print('motion: ', pid_value)
integral_error = values[1]
last_error = values[2]
motor.set_speed(direction, speed + pid_value)
motor.set_speed(other, speed - pid_value)
time.sleep(feedback_loop.delay)
dist = us.dist(us.front_trig, us.front_echo)
if dist < 40:
speed = 35 + dist
motor.set_speed('b', speed + 30)
if us.dist(us.front_trig, us.front_echo) < 15:
stop()
#checks if a qr bock is present whithin 35 cm of stand
if dist(upper) < 35:
#goes to the required range kept 1 cm extra as error due to no brakes.
if dist.upper >= 26:
motion.forward()
check_for_qr()
elif dist.upper <= 24:
motion.backward()
#stops if already in range
else:
motion.stop()
return True
#following two lines are carried out at the start of the run
last_value = [us.dist(us.right_front), us.dist(us.right_back)]
second_last_value = [us.dist(us.right_front), us.dist(us.right_back)]
while True:
right_uss_values = [us.dist(us.right_front), us.dist(us.right_back)]
# check if current= open, close and 2nd last value=close,open
if right_uss_values[0] < 10 and right_uss_values[1] > 10:
if second_last_value[0] > 10 and second_last_value[1] < 10:
box_on_right = True
#removes the case where the bot may get confused after passing the qr block
#last_value=[0,0]
#second_last_value=[0,0]
#checks if current= open, open
def move(argument):
global number_of_qr
global number_of_boxes
global box_coordinates
global qr_coordinates
global t
global time0
#global nodes
#update variables required
print('bot: (move called again) right wall dist=',
us.dist(us.right_trig, us.right_echo))
# if right is open, it will stop, turn right
if check('right'):
print('bot:right is open ')
motion.stop()
motion.turn('right')
#move forward
if callable(motion.forward):
thread_forward('right')
if not check('front'):
#if front is blocked, it will check for boxes
motion.stop()
if check_for_qr():
number_of_qr += 1
#if we are still in the first loop num will be <2
if number_of_qr == 1:
#if we have not detected all boxes in the first loop
#it will continue mapping 1st loop
t += time.time() - time0
qr_coordinates.append(t)
motion.turn('left')
time0 = time.time()
#if we have detected all boxes, move to next loop
#we have finished mapping 1st loop
elif number_of_qr == 2:
wait_for_path()
time0 = time.time()
#second loop
elif number_of_qr == 3:
t += time.time() - time0
qr_coordinates.append(t)
wait_for_path()
time0 = time.time()
elif vision.check_for_block():
t += time.time() - time0
box_coordinates.append(t)
wait_for_path()
time0 = time.time()
#if nothing found it will turn left and check the ground
else:
motion.turn('left')
if vision.ground_is_white():
t += time.time() - time0
if number_of_qr >= 2:
end_coordinates.append(t)
#nodes.append('end')
else:
start_coordinates.append(t)
#now we have to go to the qr block...
#to move to the next loop
#set t=0 for next loop
if qr_coordinates[0] > start[1] - qr_coordinates[0]:
wet_run.uturn()
if wet_run.move_till('qr', 'left'):
t = 0
else:
if wet_run.move_till('qr', 'right'):
t = 0
#elif callable(motion.forward):
#put condition for finishing mapping here
if (len(qr_coordinates) == 2) + (number_of_boxes
== 3) + (len(end_coordinates) == 1) < 3:
return move()
else:
return [
qr_coordinates, box_coordinates, start_coordinates, end_coordinates
]
def check_for_qr():
if us.dist(us.qr_trig, us.qr_echo) < 23:
return True
else:
return False
def forward(direction):
if direction='right':
trigger=us.right_trig
echo=us.right_echo
other='left'
if direction='left':
trigger=us.left_trig
echo=us.left_echo
other='right'
speed=80
global go_forward
integral_error=0
go_forward=True
last_error=feedback_loop.ideal_right_wall_dist_wall_dist-us.dist(trigger,echo)
while go_forward==True:
values=feedback_loop.adjustment_values(direction,us.dist(trigger,echo),integral_error,last_error)
pid_value= values[0]
print(pid_value)
integral_error=values[1]
last_error=values[2]
motor.set_speed(direction,motor.default_motor_speed+pid_value)
motor.set_speed(other,motor.default_motor_speed-pid_value)
time.sleep(feedback_loop.delay)
dist=us.dist(us.front_trig,us.front_echo)
if dist<40:
speed=35+dist
motor.set_speed('b',speed+30)
