# Full Obstruction #
####################
# Each front_sensor returns 1 if obstructed_threshold is surpassed
# If either one returns 1, full obstruction
front_obstructed = front_sensor(
obstructed_threshold, FRONT_ADC_INPUT_1) + front_sensor(
obstructed_threshold, FRONT_ADC_INPUT_2)
# If both motors have zero speed, full obstruction
left_right_obstructed = ((uLeft == 0) and (uRight == 0))
if ((left_right_obstructed > 0) or (front_obstructed > 0)):
# If fully obstructed, rotate in place
client_sock.send("Something's in the way. I'm relocating.")
setSpeed(ser, 0, 0, 127)
setSpeed(ser, 1, 1, 127)
else:
# If not fully obstructed, proceed with PID control
client_sock.send("Now tracking.")
setSpeed(ser, 0, 1, int(uRight * 127 * biasRight))
setSpeed(ser, 1, 1, int(uLeft * 127 * biasLeft))
time.sleep(0.1)
end = time.time()
client_sock.send("I've been tracking for %s seconds now." %
str(end - start))
# Return to scan mode
####################
# Full Obstruction #
####################
# Each front_sensor returns 1 if obstructed_threshold is surpassed
# If either one returns 1, full obstruction
front_obstructed = front_sensor(obstructed_threshold, FRONT_ADC_INPUT_1) + front_sensor(obstructed_threshold, FRONT_ADC_INPUT_2)
# If both motors have zero speed, full obstruction
left_right_obstructed = ((uLeft == 0) and (uRight == 0))
if((left_right_obstructed > 0) or (front_obstructed > 0)):
# If fully obstructed, rotate in place
client_sock.send("Something's in the way. I'm relocating.")
setSpeed(ser, 0, 0, 127)
setSpeed(ser, 1, 1, 127)
else:
# If not fully obstructed, proceed with PID control
client_sock.send("Now tracking.")
setSpeed(ser, 0, 1, int(uRight*127*biasRight))
setSpeed(ser, 1, 1, int(uLeft*127*biasLeft))
time.sleep(0.1)
end = time.time()
client_sock.send("I've been tracking for %s seconds now." % str(end - start))
# Return to scan mode
client_sock.send("Now scanning.")
import serial
from setSpeed import setSpeed
ser = serial.Serial("/dev/ttyUSB0", 115200, timeout=1)
setSpeed(ser, 0, 0, 0)
setSpeed(ser, 1, 0, 0)
uLeft, errorLeft = PID(kp, ki, kd, n, uLeft, errorLeft, ideal_value, LEFT_ADC_INPUT) # left control
uRight, errorRight = PID(kp, ki, kd, n, uRight, errorRight, ideal_value, RIGHT_ADC_INPUT) # right control
print ("uLeft: %f \n") % (uLeft)
print ("uRight: %f \n") % (uRight)
print ("errorLeft: %s \n") % (str(errorLeft))
print ("errorRight: %s \n") % (str(errorRight))
print ("n: %i \n") % (n)
print "--------------------------\n"
n = n + 1
front_obstructed = front_sensor(obstructed_threshold, FRONT_ADC_INPUT_1) + front_sensor(obstructed_threshold, FRONT_ADC_INPUT_2)
left_right_obstructed = ((uLeft == 0) and (uRight == 0))
if((left_right_obstructed > 0) or (front_obstructed > 0)):
setSpeed(ser, 0, 0, 127)
setSpeed(ser, 1, 1, 127)
front_obstructed = front_sensor(obstructed_threshold, FRONT_ADC_INPUT_1) + front_sensor(obstructed_threshold, FRONT_ADC_INPUT_2)
left_right_obstructed = front_sensor(obstructed_threshold, RIGHT_ADC_INPUT) + front_sensor(obstructed_threshold, LEFT_ADC_INPUT)
# time.sleep(0.5)
else:
setSpeed(ser, 0, 1, int(uRight*127))
setSpeed(ser, 1, 1, int(uLeft*127))
"""
setSpeed(ser, 0, 1, int(uRight*127))
setSpeed(ser, 1, 1, int(uLeft*127))
"""
time.sleep(0.1)
import serial
from setSpeed import setSpeed
ser = serial.Serial('/dev/ttyUSB0', 115200, timeout=1)
setSpeed(ser, 0, 0, 0)
setSpeed(ser, 1, 0, 0)
