hsv_roi = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
mask = cv2.inRange(hsv_roi, np.array((0., 60.,32.)), np.array((180.,255.,255.)))
roi_hist = cv2.calcHist([hsv_roi],[0],mask,[180],[0,180])
cv2.normalize(roi_hist,roi_hist,0,255,cv2.NORM_MINMAX)
# Setup the termination criteria, either 10 iteration or move by atleast 1 pt
term_crit = ( cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 10, 1 )
direct='w'
lastdist=0
while(1):
ret ,frame = cap.read()
if ret == True:
hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
dst = cv2.calcBackProject([hsv],[0],roi_hist,[0,180],1)
motors.motor_move(direct,4)
motors2.motor_move(direct,4)
lineSensors.check1()
lineSensors.check2()
lineSensors.check3()
lineSensors.check4()
# apply meanshift to get the new location
ret, track_window = cv2.meanShift(dst, track_window, term_crit)
# Draw it on image
x,y,w,h = track_window
dist= (x+w)/2
avgd=lastdist-dist
while(avgd> 50 or avgd<-50):
x,y,w,h = track_window
avgd = dist-x
dist=x
import time
from lineSense2 import lineSensor_Class
from minisumo_motorcontrol2 import Motors_Class
from longrangemethod import longrange_Class
from shortrangemethod import shortrange_Class
lineSensors= lineSensor_Class()
motors= Motors_Class()
longrange= longrange_Class()
shortrange= shortrange_Class()
motors.motor_move("w",4)
while(True):
if(1==lineSensors.check1()):
motors.motor_move("x",0)
time.sleep(1)
motors.motor_move("s",4)
elif(1==lineSensors.check2()):
motors.motor_move('x',0)
time.sleep(1)
motors.motor_move('s',4)
elif(1==lineSensors.check3()):
motors.motor_move('x',0)
time.sleep(1)
motors.motor_move('s',4)
elif(1==lineSensors.check4()):
motors.motor_move('x',0)
time.sleep(1)
motors.motor_move('s',4)
elif(shortrange.rngsens()>3):
motors.motor_move('x',0)
# print('\nL3 NOT WORKING')
# else:
# print ('l3 working')
# print('place br corner on line')
# pressToContinue()
if l4 == 1:
print("\nL4 NOT WORKING")
else:
print("l4 working")
# Motors
print("\nTESTING MOTORS NOW")
pressToContinue()
# FRONT MOTOR TEST
motors.motor_move("w", 4)
ax, ay, vx, vy, r_mx, r_my = Adafruit_LSM303.mat_accel()
print("ax = ", ax)
if ax < 0 or ax > 0.25:
print("\nAX ERROR")
print("ay = ", ay)
if ay < 0 or ay > 0.25:
print("\nAY ERROR")
print("vx = ", vx)
print("vy = ", vy)
print("r_mx = ", r_mx)
print("r_my =", r_my)
pressToContinue()
motors.motor_move("x", 0)
# BACK MOTOR TEST
import time
from lineSense2 import lineSensor_Class
from minisumo_motorcontrol2 import Motors_Class
from longrangemethod import longrange_Class
from shortrangemethod import shortrange_Class
lineSensors = lineSensor_Class()
motors = Motors_Class()
longrange = longrange_Class()
shortrange = shortrange_Class()
motors.motor_move("w", 4)
while (True):
if (1 == lineSensors.check1()):
motors.motor_move("x", 0)
time.sleep(1)
motors.motor_move("s", 4)
elif (1 == lineSensors.check2()):
motors.motor_move('x', 0)
time.sleep(1)
motors.motor_move('s', 4)
elif (1 == lineSensors.check3()):
motors.motor_move('x', 0)
time.sleep(1)
motors.motor_move('s', 4)
elif (1 == lineSensors.check4()):
motors.motor_move('x', 0)
time.sleep(1)
motors.motor_move('s', 4)
elif (shortrange.rngsens() > 3):
motors.motor_move('x', 0)
moving('x')
motors.motor_move('a', 2)
else:
break
else:
moving('x')
motors.motor_move('a', 4)
else:
break
def moving(dir):
if dir = 'x':
motors.motor_move('x', 0)
motors2.motor_move('x', 0)
if dir = 'a':
motors.motor_move('a', 4)
motors2.motor_move('a',4)
if dir = 's':
motors.motor_move('s',4)
motors2.motor_move('s',4)
if dir = 'd':
motors.motor_move('d', 4)
motors2.motor_move('d',4)
if dir = 'w':
motors.motor_move('w',4)
motors2.motor_move('w', 4)
##MAIN LOOP
while True:
short = shortrange.rngsens
while short > 2 and short < 15:
from lineSense2 import lineSensor_Class
from longrangemethod import longrange_Class
from shortrangemethod import shortrange_Class
from minisumo_motorcontrol2 import Motors_Class
from minisumo_motorcontrol3 import Motors_Class2
from accelerometer import Accel
import time
lineSensors= lineSensor_Class()
Accel1 = Accel()
motor1= Motors_Class()
motor2 = Motors_Class2()
longrange= longrange_Class()
shortrange= shortrange_Class()
try:
while True:
motor1.motor_move('a', 3)
motor2.motor_move('a', 3)
shortrange.rngsens()
lineSensors.check1()
lineSensors.check2()
lineSensors.check3()
lineSensors.check4()
longrange.rangesens()
except KeyboardInterrupt:
GPIO.cleanup()
from minisumo_motorcontrol2 import Motors_Class
from minisumo_motorcontrol3 import Motors_Class2
motors=Motors_Class()
motors2=Motors_Class2()
while(1):
direct = raw_input()
motors.motor_move(direct,6)
motors2.motor_move(direct,6)
from lineSense2 import lineSensor_Class
from longrangemethod import longrange_Class
from shortrangemethod import shortrange_Class
from minisumo_motorcontrol2 import Motors_Class
from minisumo_motorcontrol3 import Motors_Class2
from accelerometer import Accel
import time
lineSensors = lineSensor_Class()
Accel1 = Accel()
motor1 = Motors_Class()
motor2 = Motors_Class2()
longrange = longrange_Class()
shortrange = shortrange_Class()
try:
while True:
motor1.motor_move('a', 3)
motor2.motor_move('a', 3)
shortrange.rngsens()
lineSensors.check1()
lineSensors.check2()
lineSensors.check3()
lineSensors.check4()
longrange.rangesens()
except KeyboardInterrupt:
GPIO.cleanup()
from minisumo_motorcontrol2 import Motors_Class
from minisumo_motorcontrol3 import Motors_Class2
motors = Motors_Class()
motors2 = Motors_Class2()
while (1):
direct = raw_input()
motors.motor_move(direct, 6)
motors2.motor_move(direct, 6)
