def discovery():
motorTest.turnRight()
time.sleep(0.3)
motorTest.forward(150, 150)
# time.sleep(0.3)
motorTest.turnLeft()
# time.sleep(0.3)
# motorTest.rev()
time.sleep(0.002)
def findColor(x, y, area):
global flag2
global servoPos
if area < 50:
flag2 = 0
if x > 1 and y > 1 and area > 5000: #was 5000
print("*** X is: " + str(x) + "*** Y is: " + str(y))
point = (x, y)
# print("AREA OF COLOR IS: "+ str(area))
# cv2.circle(frame, point, 10, (255,0,0), 0)
# if x < 70 and y < 75:
# motorTest.forward(50, 220-x)
# elif x > 90 and y < 75:
# motorTest.forward(70+x, 50)
# elif x > 70 and x < 90 and y < 75:
# motorTest.forward(250, 250)
if servoPos < 60:
motorTest.forward(50, 200)
elif servoPos > 68:
motorTest.forward(200, 50)
else:
motorTest.forward(200, 200)
if y > 175 or area > 15000:
motorTest.off()
if flag2 == 0:
os.system('mpg123 -f 57000 -m clip.mp3 &')
flag2 = 1
def navigate():
rangeL = us.rangeL()
rangeR = us.rangeR()
if rangeL < 20 and rangeL > 8 or rangeL < 6:
motorTest.avoidReverseL()
elif rangeR < 20 and rangeR > 8 or rangeR < 6:
motorTest.avoidReverseR()
if rangeL < 100 or rangeR < 100:
if rangeL - rangeR > 12:
motorTest.forward(75, 290)
elif rangeL - rangeR < -12:
motorTest.forward(290, 75)
else:
motorTest.forward(275, 275)
def findColor(x, y):
if x > 1 and y > 1:
point = (x, y)
cv2.circle(frame, point, 10, (255, 0, 0), 0)
if x < 70 and y < 75:
motorTest.forward(50, 220 - x)
print("Color is to the left. Offset of: " + str(220 - x) +
" speed.")
elif x > 90 and y < 75:
motorTest.forward(70 + x, 50)
print("Color is to the right. Offset of: " + str(70 + x) +
" speed.")
elif x > 70 and x < 90 and y < 75:
motorTest.forward(250, 250)
print("Color is in center")
if y > 75 and y < 95:
motorTest.off()
time.sleep(0.1)
elif y > 95 and y < 150:
motorTest.reverse(180)
time.sleep(0.3)
elif x <= 1 and y <= 1:
motorTest.off()
import motorTest
direction = ''
speed = 10
ans = ''
while direction != 'Q':
direction = raw_input(
'Enter (F)orward, (B)ackward, (L)eft, (R)ight, (C)reep, (V)ariable random, or (Q)uit: '
)
if direction == 'B':
motorTest.reverse()
elif direction == 'F':
motorTest.forward()
elif direction == 'L':
motorTest.turnLeft()
elif direction == 'R':
motorTest.turnRight()
elif direction == 'C':
speed = raw_input('Enter desired RPM: ')
motorTest.creep(int(speed))
elif direction == 'V':
motorTest.randomForward()
else:
print('Incorrect input. Answer better. ')