def radar_scan():
global pwm1_pos
RGB.cyan()
scan_result = "U: "
scan_speed = 1
if pwm1_direction:
pwm1_pos = pwm1_max
pwm.set_pwm(1, 0, pwm1_pos)
time.sleep(0.5)
scan_result += str(ultra.checkdist())
scan_result += " "
while pwm1_pos > pwm1_min:
pwm1_pos -= scan_speed
pwm.set_pwm(1, 0, pwm1_pos)
scan_result += str(ultra.checkdist())
scan_result += " "
pwm.set_pwm(1, 0, pwm1_init)
pwm1_pos = pwm1_init
else:
pwm1_pos = pwm1_min
pwm.set_pwm(1, 0, pwm1_pos)
time.sleep(0.5)
scan_result += str(ultra.checkdist())
scan_result += " "
while pwm1_pos < pwm1_max:
pwm1_pos += scan_speed
pwm.set_pwm(1, 0, pwm1_pos)
scan_result += str(ultra.checkdist())
scan_result += " "
pwm.set_pwm(1, 0, pwm1_init)
pwm1_pos = pwm1_init
RGB.both_on()
return scan_result
def ultra_send_client():
global distance
distance = 0
ultra_IP = addr[0]
ultra_PORT = 2257 #Define port serial
ultra_ADDR = (ultra_IP, ultra_PORT)
ultra_Socket = socket.socket(
socket.AF_INET, socket.SOCK_STREAM) #Set connection value for socket
ultra_Socket.connect(ultra_ADDR)
print(ultra_ADDR)
while 1:
while ultrasonicMode:
try:
if not FindColorMode:
distance = ultra.checkdist()
ultra_Socket.send(str(round(distance, 2)).encode())
if direction_command == 'forward' and distance < 0.3:
move.motorStop()
time.sleep(0.5)
continue
fpv.UltraData(round(ultra.checkdist(), 2))
time.sleep(0.2)
except:
pass
time.sleep(0.5)
def radarScan(self): global pwm0_pos scan_speed = 3 result = [] if pwm0_direction: pwm0_pos = pwm0_max pwm.set_pwm(1, 0, pwm0_pos) time.sleep(0.8) while pwm0_pos>pwm0_min: pwm0_pos-=scan_speed pwm.set_pwm(1, 0, pwm0_pos) dist = ultra.checkdist() if dist > 20: continue theta = 180 - (pwm0_pos-100)/2.55 # +30 deviation result.append([dist, theta]) else: pwm0_pos = pwm0_min pwm.set_pwm(1, 0, pwm0_pos) time.sleep(0.8) while pwm0_pos<pwm0_max: pwm0_pos+=scan_speed pwm.set_pwm(1, 0, pwm0_pos) dist = ultra.checkdist() if dist > 20: continue theta = (pwm0_pos-100)/2.55 result.append([dist, theta]) pwm.set_pwm(1, 0, pwm0_init) return result
def automaticProcessing(self):
global mark_automatic
# print('automaticProcessing')
if self.rangeKeep/3 > ultra.checkdist():
move.move(100, 'backward', 'no', 0.5)
if mark_automatic != 1:
# print(ultra.checkdist())
print("backward")
mark_automatic = 1
elif self.rangeKeep > ultra.checkdist():
move.move(100, 'no', 'left', 0.5)
if mark_automatic != 2:
# print(ultra.checkdist())
print("left")
else:
move.move(100, 'forward', 'no', 0.5)
if mark_automatic != 3:
# print(ultra.checkdist())
print("forward")
mark_automatic = 3
time.sleep(0.1)
if self.functionMode == 'none':
move.move(80, 'no', 'no', 0.5)
print("stop")
def radar_scan(): global pwm0_pos scan_result = 'U: ' scan_speed = 1 if pwm0_direction: pwm0_pos = pwm0_max pwm.set_pwm(0, 0, pwm0_pos) time.sleep(0.5) scan_result += str(ultra.checkdist()) scan_result += ' ' while pwm0_pos>pwm0_min: pwm0_pos-=scan_speed pwm.set_pwm(0, 0, pwm0_pos) scan_result += str(ultra.checkdist()) scan_result += ' ' pwm.set_pwm(0, 0, pwm0_init) else: pwm0_pos = pwm0_min pwm.set_pwm(0, 0, pwm0_pos) time.sleep(0.5) scan_result += str(ultra.checkdist()) scan_result += ' ' while pwm0_pos<pwm0_max: pwm0_pos+=scan_speed pwm.set_pwm(0, 0, pwm0_pos) scan_result += str(ultra.checkdist()) scan_result += ' ' pwm.set_pwm(0, 0, pwm0_init) return scan_result
def turnLeftWhenObstacle():
m.motorStart(1, m.Dir_forward, 80)
while 1:
if u.checkdist() < dist:
t.left()
while u.checkdist() < dist:
pass
t.middle()
def autoDect(speed):
move.motorStop()
servo.ahead()
time.sleep(0.3)
getMiddle = ultra.checkdist()
print('M%f' % getMiddle)
servo.ahead()
servo.lookleft(100)
time.sleep(0.3)
getLeft = ultra.checkdist()
print('L%f' % getLeft)
servo.ahead()
servo.lookright(100)
time.sleep(0.3)
getRight = ultra.checkdist()
print('R%f' % getRight)
if getMiddle < range_min and min(getLeft, getRight) > range_min:
if random.randint(0, 1):
servo.turnLeft()
else:
servo.turnRight()
move.move(speed, 'forward')
time.sleep(0.5)
move.motorStop()
elif getLeft < range_min and min(getMiddle, getRight) > range_min:
servo.turnRight(0.7)
move.move(speed, 'forward')
time.sleep(0.5)
move.motorStop()
elif getRight < range_min and min(getMiddle, getLeft) > range_min:
servo.turnLeft(0.7)
move.move(speed, 'forward')
time.sleep(0.5)
move.motorStop()
elif max(getLeft, getMiddle) < range_min and getRight > range_min:
servo.turnRight()
move.move(speed, 'forward')
time.sleep(0.5)
move.motorStop()
elif max(getMiddle, getRight) < range_min and getLeft > range_min:
servo.turnLeft()
move.move(speed, 'forward')
time.sleep(0.5)
move.motorStop()
elif max(getLeft, getMiddle, getRight) < range_min:
move.move(speed, 'backward')
time.sleep(0.5)
move.motorStop()
else:
servo.turnMiddle()
move.move(speed, 'forward')
time.sleep(0.5)
move.motorStop()
def automaticProcessing(self):
print('automaticProcessing')
if self.rangeKeep / 3 > ultra.checkdist():
move.move(100, 'backward', 'no', 0.5)
elif self.rangeKeep > ultra.checkdist():
move.move(100, 'no', 'left', 0.5)
else:
move.move(100, 'forward', 'no', 0.5)
time.sleep(0.1)
if self.functionMode == 'none':
move.move(80, 'no', 'no', 0.5)
def distRedress(self):
mark = 0
distValue = ultra.checkdist() * 100
while True:
distValue = ultra.checkdist() * 100
if distValue > 900:
mark += 1
elif mark > 5 or distValue < 900:
break
print(distValue)
return round(distValue, 2)
def roomba_pattern(): speed_set = 100 BUFFER_DISTANCE = 0.5 # meters while True: move(speed_set, 'forward', '') ultradist = round(ultra.checkdist(),2) while ultradist > BUFFER_DISTANCE: time.sleep(0.5) ultradist = round(ultra.checkdist(),2) move(speed_set, 'no', 'right') time.sleep(1) # or random
def turnLeftWhenObstacle():
m.motorStart(1, m.Dir_forward, 90)
while 1:
if u.checkdist() < dist:
#m.motorStop()
t.left()
#m.motorStart(1, m.Dir_forward, 100)
while u.checkdist() < dist:
pass
t.middle()
while 1:
if u.checkdist() < dist:
m.motorStop()
break
def autoDect(speed):
move.motorStop()
servo.ahead()
time.sleep(0.3)
getMiddle = ultra.checkdist()
print('M%f' % getMiddle)
servo.ahead()
servo.lookleft(100)
time.sleep(0.3)
getLeft = ultra.checkdist()
print('L%f' % getLeft)
servo.ahead()
servo.lookright(100)
time.sleep(0.3)
getRight = ultra.checkdist()
print('R%f' % getRight)
if getMiddle >= max(getLeft, getRight):
if getMiddle > range_min:
move.move(speed, 'forward')
time.sleep(0.5)
move.motorStop()
else:
move.move(speed, 'backward')
time.sleep(0.5)
elif getLeft > max(getMiddle, getRight):
servo.ahead()
servo.turnLeft()
servo.lookleft(100)
if getLeft > range_min:
move.move(speed, 'forward')
time.sleep(0.5)
move.motorStop()
else:
move.move(speed, 'backward')
time.sleep(0.5)
elif getRight > max(getMiddle, getLeft):
servo.ahead()
servo.turnRight()
servo.lookright(100)
if getRight > range_min:
move.move(speed, 'forward')
time.sleep(0.5)
move.motorStop()
else:
move.move(speed, 'backward')
time.sleep(0.5)
def automaticProcessing(self):
print("automaticProcessing")
scGear.moveAngle(2, 0)
if self.scanPos == 1:
pwm.set_pwm(self.scanServo, 0, pwm1_init + self.scanRange)
time.sleep(0.3)
self.scanList[0] = ultra.checkdist()
elif self.scanPos == 2:
pwm.set_pwm(self.scanServo, 0, pwm1_init)
time.sleep(0.3)
self.scanList[1] = ultra.checkdist()
elif self.scanPos == 3:
pwm.set_pwm(self.scanServo, 0, pwm1_init - self.scanRange)
time.sleep(0.3)
self.scanList[2] = ultra.checkdist()
self.scanPos = self.scanPos + self.scanDir
if self.scanPos > self.scanNum or self.scanPos < 1:
if self.scanDir == 1:
self.scanDir = -1
elif self.scanDir == -1:
self.scanDir = 1
self.scanPos = self.scanPos + self.scanDir * 2
print(self.scanList)
if min(self.scanList) < self.rangeKeep:
if self.scanList.index(min(self.scanList)) == 0:
scGear.moveAngle(2, -30)
elif self.scanList.index(min(self.scanList)) == 1:
if self.scanList[0] < self.scanList[2]:
scGear.moveAngle(2, -45)
else:
scGear.moveAngle(2, 45)
elif self.scanList.index(min(self.scanList)) == 2:
scGear.moveAngle(2, 30)
if (
max(self.scanList) < self.rangeKeep
or min(self.scanList) < self.rangeKeep / 3
):
move.motor_left(1, 1, 80)
move.motor_right(1, 1, 80)
else:
# move along
move.motor_left(1, 0, 80)
move.motor_right(1, 0, 80)
pass
def monitor_distance(LED, min_distance_m=0.1):
'''Monitor distance using ultasonic module and change LED based on distance'''
while True:
current_distance = round(ultra.checkdist(), 2)
if_debug_log("distance: " + str(current_distance))
if current_distance > (2 * min_distance_m):
#distance far, normal white LED
LED.colorWipe(Color(255, 255, 255))
if_debug_log("distance: " + str(current_distance) +
" --> normal, LED white")
elif (current_distance > (min_distance_m) and current_distance <=
(2 * min_distance_m)):
#getting closer, LED yellow
LED.colorWipe(Color(255, 255, 0))
if_debug_log("distance: " + str(current_distance) +
" --> getting closer, LED yellow")
elif current_distance <= min_distance_m:
#too close, LED red
LED.colorWipe(Color(255, 0, 0))
if_debug_log("distance: " + str(current_distance) +
" --> too close, LED red")
def dis_scan_thread(): #Get Ultrasonic scan distance
global dis_data
while 1:
while dis_scan:
dis_data = ultra.checkdist()
time.sleep(0.2)
time.sleep(0.2)
def scan(): #TODO: move to ultra module
'''Ultrasonic Scanning'''
#TODO: better docstring here
#??? why is this global if it gets reset and reterned in the function
global dis_dir
#This value determine the speed of scanning,the greater the faster
ultraScanSpeed = 3 #TODO: make global
dis_dir = []
#??? why not just go to left max...
turn.ultra_turn(hoz_mid) #Ultrasonic point forward
turn.ultra_turn(look_left_max) #Ultrasonic point Left,prepare to scan
# Make a mark so that the client would know it is a list
#??? it can't tell... #??? mixed types in list
dis_dir = ['list']
time.sleep(0.5) #Wait for the Ultrasonic to be in position
#??? why is this named cat_2
cat_2 = look_left_max
#??? why don't we want warnings... #??? should this be global
GPIO.setwarnings(False) #Or it may print warnings
while cat_2 > look_right_max: #Scan,from left to right
turn.ultra_turn(cat_2)
cat_2 -= ultraScanSpeed # Move to next position
#Get a distance of a certern direction
new_scan_data = round(ultra.checkdist(), 2)
#Put that distance value into a list,
#and save it as String-Type for future transmission
#??? why a string?
dis_dir.append(str(new_scan_data))
turn.ultra_turn(hoz_mid)
return dis_dir
def dis_scan_thread(): #??? why thread this at all, drive and scan?
'''Get Ultrasonic scan distance'''
global dis_data
while 1:
while dis_scan: #??? double loop to stop thread from closing when not running
dis_data = ultra.checkdist()
time.sleep(0.2)
time.sleep(0.2)
def keepDisProcessing(self):
print('keepDistanceProcessing')
distanceGet = ultra.checkdist()
if distanceGet > (self.rangeKeep/2+0.1):
move.move(100, 'forward', 'no', 0.5)
elif distanceGet < (self.rangeKeep/2-0.1):
move.move(100, 'forward', 'no', 0.5)
else:
move.motorStop()
def automaticProcessing(self):
dist = ultra.checkdist()
if dist < distanceCheak:
robot.robotCtrl.moveStart(100, 'no', 'left')
time.sleep(turningKeep)
else:
robot.robotCtrl.moveStart(100, 'forward', 'no')
time.sleep(0.1)
def automaticProcessing(self):
print('automaticProcessing')
pwm.set_pwm(2, 0, pwm2_init)
if self.scanPos == 1:
pwm.set_pwm(self.scanServo, 0, pwm1_init - self.scanRange)
time.sleep(0.3)
self.scanList[0] = ultra.checkdist()
elif self.scanPos == 2:
pwm.set_pwm(self.scanServo, 0, pwm1_init)
time.sleep(0.3)
self.scanList[1] = ultra.checkdist()
elif self.scanPos == 3:
pwm.set_pwm(self.scanServo, 0, pwm1_init + self.scanRange)
time.sleep(0.3)
self.scanList[2] = ultra.checkdist()
self.scanPos = self.scanPos + self.scanDir
if self.scanPos > self.scanNum or self.scanPos < 1:
if self.scanDir == 1: self.scanDir = -1
elif self.scanDir == -1: self.scanDir = 1
self.scanPos = self.scanPos + self.scanDir * 2
print(self.scanList)
if min(self.scanList) < self.rangeKeep:
if self.scanList.index(min(self.scanList)) == 0:
pwm.set_pwm(self.turnServo, 0,
pwm0_init + int(self.turnWiggle / 3.5))
elif self.scanList.index(min(self.scanList)) == 1:
if self.scanList[0] < self.scanList[2]:
pwm.set_pwm(self.turnServo, 0, pwm0_init + self.turnWiggle)
else:
pwm.set_pwm(self.turnServo, 0, pwm0_init - self.turnWiggle)
elif self.scanList.index(min(self.scanList)) == 2:
pwm.set_pwm(self.turnServo, 0,
pwm0_init - int(self.turnWiggle / 3.5))
if max(self.scanList) < self.rangeKeep or min(
self.scanList) < self.rangeKeep / 3:
move.move(80, 'backward', 'no', 0.5)
else:
#move along
move.move(80, 'forward', 'no', 0.5)
pass
def ultra_send_client():
ultra_IP = addr[0]
ultra_PORT = 2257 #Define port serial
ultra_ADDR = (ultra_IP, ultra_PORT)
ultra_Socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) #Set connection value for socket
ultra_Socket.connect(ultra_ADDR)
print(ultra_ADDR)
while 1:
while ultrasonicMode:
try:
if not FindColorMode:
ultra_Socket.send(str(round(ultra.checkdist(),2)).encode())
time.sleep(0.5)
continue
fpv.UltraData(round(ultra.checkdist(),2))
time.sleep(0.2)
except:
pass
time.sleep(0.5)
def capture_thread(self):
footage_socket = setup_pub(5555)
mask_socket = setup_pub(5556)
# for watchdog
global avg, motionCounter, lastMotionCaptured, timestamp
avg = None
motionCounter = 0
#time.sleep(4)
lastMotionCaptured = datetime.datetime.now()
timestamp = datetime.datetime.now()
num_frames = 0
for frame in camera.capture_continuous(rawCapture,
format="bgr",
use_video_port=True):
while self.stream_on == False: # TODO need way to flush buffer ...?
time.sleep(0.3)
rawCapture.truncate(0)
continue
frame_image = frame.array
if FindColorMode:
frame_image, mask = self.find_color_fnc(frame_image)
if WatchDogMode:
frame_image = self.watchdog_mode_fnc(frame_image)
cv2.line(frame_image, (300, 240), (340, 240), (128, 255, 128), 1)
cv2.line(frame_image, (320, 220), (320, 260), (128, 255, 128), 1)
dist = str(ultra.checkdist())
cv2.putText(frame_image, dist, (340, 260),
cv2.FONT_HERSHEY_COMPLEX, 1, (255, 255, 255), 1)
timestamp = datetime.datetime.now()
send_mask = False
if send_mask: # TODO probs should do socket creation in init ..
print('sending mask frame')
send_frame(mask, mask_socket)
blend_images = FindColorMode
if blend_images:
mask = cv2.cvtColor(mask, cv2.COLOR_GRAY2BGR)
frame_image = cv2.addWeighted(frame_image, 1.0, mask, 0.5, 0.0)
send_frame(frame_image, footage_socket)
print('sent frame {}'.format(num_frames))
num_frames += 1
#time.sleep(1)
rawCapture.truncate(0)
def keepDisProcessing(self):
print("keepDistanceProcessing")
distanceGet = ultra.checkdist()
if distanceGet > (self.rangeKeep / 2 + 0.1):
move.motor_left(1, 0, 80)
move.motor_right(1, 0, 80)
elif distanceGet < (self.rangeKeep / 2 - 0.1):
move.motor_left(1, 1, 80)
move.motor_right(1, 1, 80)
else:
move.motorStop()
def keepDProcessing(self):
dist = ultra.checkdist()
if dist < distanceCheak - 0.1:
robot.robotCtrl.moveStart(100, 'backward', 'no')
elif dist > distanceCheak + 0.1:
robot.robotCtrl.moveStart(100, 'forward', 'no')
else:
robot.robotCtrl.moveStop()
time.sleep(0.1)
def _publish_sonar(self):
if self._sonar_pub.get_num_connections() == 0:
return
now = rospy.Time.now()
sonar = Range()
sonar.header.stamp = now
sonar.radiation_type = 0
sonar.field_of_view = 0.2618
sonar.min_range = 0.02
sonar.max_range = 4.0
sonar.range = ultra.checkdist()
self._sonar_pub.publish(sonar)
def scan_rev(): #Ultrasonic Scanning
global dis_dir
dis_dir = []
turn.ultra_turn(hoz_mid) #Ultrasonic point forward
turn.ultra_turn(look_right_max) #Ultrasonic point Left,prepare to scan
dis_dir=['list'] #Make a mark so that the client would know it is a list
time.sleep(0.5) #Wait for the Ultrasonic to be in position
cat_2=look_right_max #Value of left-position
GPIO.setwarnings(False) #Or it may print warnings
while cat_2<look_left_max: #Scan,from left to right
turn.ultra_turn(cat_2)
cat_2 += 3 #This value determine the speed of scanning,the greater the faster
new_scan_data=round(ultra.checkdist(),2) #Get a distance of a certern direction
dis_dir.append(str(new_scan_data)) #Put that distance value into a list,and save it as String-Type for future transmission
turn.ultra_turn(hoz_mid) #Ultrasonic point forward
return dis_dir
def move(speed, direction, turn, radius=0.6, CRASH_OVERRIDE=False): # 0 < radius <= 1 speed = 100 if direction == 'forward': if turn == 'left': motor_left(0, left_backward, int(speed*radius)) motor_right(1, right_forward, speed) elif turn == 'right': motor_left(1, left_forward, speed) motor_right(0, right_backward, int(speed*radius)) else: if CRASH_OVERRIDE: ultradist = round(ultra.checkdist(),2) if ultradist < 0.5: motorStop() else: motor_left(1, left_forward, 100) motor_right(1, right_forward, 100) elif direction == 'backward': if turn == 'left': motor_left(0, left_forward, int(speed*radius)) motor_right(1, right_backward, speed) elif turn == 'right': motor_left(1, left_backward, speed) motor_right(0, right_forward, int(speed*radius)) else: motor_left(1, left_backward, speed) motor_right(1, right_backward, speed) elif direction == 'no': if turn == 'left': motor_left(1, left_backward, 100) motor_right(1, right_forward, 100) elif turn == 'right': motor_left(1, left_forward, 100) motor_right(1, right_backward, 100) else: motorStop() else: pass
def find_color_fnc(self, frame_image):
####>>>OpenCV Start<<<####
hsv = cv2.cvtColor(frame_image, cv2.COLOR_BGR2HSV)
mask = cv2.inRange(hsv, self.colorLower, self.colorUpper)
mask = cv2.erode(mask, None, iterations=2)
mask = cv2.dilate(mask, None, iterations=2)
cnts = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_SIMPLE)[-2]
center = None
if len(cnts) > 0:
cv2.putText(frame_image, 'Target Detected', (40, 60), font, 0.5,
(255, 255, 255), 1, cv2.LINE_AA)
c = max(cnts, key=cv2.contourArea)
((x, y), radius) = cv2.minEnclosingCircle(c)
M = cv2.moments(c)
center = (int(M["m10"] / M["m00"]), int(M["m01"] / M["m00"]))
X = int(x)
Y = int(y)
if radius > 10:
cv2.rectangle(frame_image, (int(x - radius), int(y + radius)),
(int(x + radius), int(y - radius)),
(255, 255, 255), 1)
if Y < (240 - tor):
error = (240 - Y) / 5
outv = int(round((pid.GenOut(error)), 0))
servo.camera_ang('lookup', outv)
Y_lock = 0
elif Y > (240 + tor):
error = (Y - 240) / 5
outv = int(round((pid.GenOut(error)), 0))
servo.camera_ang('lookdown', outv)
Y_lock = 0
else:
Y_lock = 1
if X < (320 - tor * 3):
move.move(70, 'no', 'left', 0.6)
time.sleep(0.1)
#move.motorStop()
X_lock = 0
elif X > (330 + tor * 3):
move.move(70, 'no', 'right', 0.6)
time.sleep(0.1)
#move.motorStop()
X_lock = 0
else:
move.motorStop()
X_lock = 1
if X_lock == 1: # and Y_lock == 1:
UltraData = ultra.checkdist()
if UltraData > 3:
move.motorStop()
elif UltraData > 0.5:
move.move(70, 'forward', 'no', 0.6)
elif UltraData < 0.4:
move.move(70, 'backward', 'no', 0.6)
print(UltraData)
else:
move.motorStop()
else:
cv2.putText(frame_image, 'Target Detecting', (40, 60), font, 0.5,
(255, 255, 255), 1, cv2.LINE_AA)
move.motorStop()
for i in range(1, len(pts)):
if pts[i - 1] is None or pts[i] is None:
continue
thickness = int(np.sqrt(args["buffer"] / float(i + 1)) * 2.5)
cv2.line(frame_image, pts[i - 1], pts[i], (0, 0, 255), thickness)
####>>>OpenCV Ends<<<####
return frame_image, mask
def move_forward():
move.move(speed_set, DIRECTION_FORWARD, TURN_NO, rad)
def turn_left():
# move.move(speed_set, DIRECTION_FORWARD, TURN_LEFT, rad)
move.motor_left(1, 0, rot_speed)
move.motor_right(1, 0, rot_speed)
sleep_done = 0
move.setup()
while 1:
# Measure distance
distance = ultra.checkdist()
print("distance=" + str(distance))
if distance >= 0.7:
sleep_done = 0
# if there is a way, go
move_forward()
else:
# if there is no way, turn
if sleep_done == 0:
move.motorStop()
time.sleep(1)
sleep_done = 1
turn_left()
time.sleep(0.1)
def capture_thread(self, IPinver):
global frame_image, camera
ap = argparse.ArgumentParser() #OpenCV initialization
ap.add_argument("-b",
"--buffer",
type=int,
default=64,
help="max buffer size")
args = vars(ap.parse_args())
pts = deque(maxlen=args["buffer"])
font = cv2.FONT_HERSHEY_SIMPLEX
camera = picamera.PiCamera()
camera.resolution = (640, 480)
camera.framerate = 20
rawCapture = PiRGBArray(camera, size=(640, 480))
context = zmq.Context()
footage_socket = context.socket(zmq.PUB)
print(IPinver)
footage_socket.connect('tcp://%s:5555' % IPinver)
avg = None
motionCounter = 0
lastMovtionCaptured = datetime.datetime.now()
for frame in camera.capture_continuous(rawCapture,
format="bgr",
use_video_port=True):
frame_image = frame.array
timestamp = datetime.datetime.now()
if FindColorMode:
####>>>OpenCV Start<<<####
hsv = cv2.cvtColor(frame_image, cv2.COLOR_BGR2HSV)
mask = cv2.inRange(hsv, colorLower, colorUpper) #1
mask = cv2.erode(mask, None, iterations=2)
mask = cv2.dilate(mask, None, iterations=2)
cnts = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_SIMPLE)[-2]
center = None
if len(cnts) > 0:
cv2.putText(frame_image, 'Target Detected', (40, 60), font,
0.5, (255, 255, 255), 1, cv2.LINE_AA)
c = max(cnts, key=cv2.contourArea)
((x, y), radius) = cv2.minEnclosingCircle(c)
M = cv2.moments(c)
center = (int(M["m10"] / M["m00"]),
int(M["m01"] / M["m00"]))
X = int(x)
Y = int(y)
if radius > 10:
cv2.rectangle(frame_image,
(int(x - radius), int(y + radius)),
(int(x + radius), int(y - radius)),
(255, 255, 255), 1)
if Y < (240 - tor):
error = (240 - Y) / 5
outv = int(round((pid.GenOut(error)), 0))
servo.up(outv)
Y_lock = 0
elif Y > (240 + tor):
error = (Y - 240) / 5
outv = int(round((pid.GenOut(error)), 0))
servo.down(outv)
Y_lock = 0
else:
Y_lock = 1
if X < (320 - tor * 3):
error = (320 - X) / 5
outv = int(round((pid.GenOut(error)), 0))
servo.lookleft(outv)
servo.turnLeft(coe_Genout(error, 64))
X_lock = 0
elif X > (330 + tor * 3):
error = (X - 240) / 5
outv = int(round((pid.GenOut(error)), 0))
servo.lookright(outv)
servo.turnRight(coe_Genout(error, 64))
X_lock = 0
else:
move.motorStop()
X_lock = 1
if X_lock == 1 and Y_lock == 1:
switch.switch(1, 1)
switch.switch(2, 1)
switch.switch(3, 1)
moveCtrl(ultra.checkdist(), back_R, forward_R)
else:
move.motorStop()
switch.switch(1, 0)
switch.switch(2, 0)
switch.switch(3, 0)
else:
cv2.putText(frame_image, 'Target Detecting', (40, 60),
font, 0.5, (255, 255, 255), 1, cv2.LINE_AA)
move.motorStop()
####>>>OpenCV Ends<<<####
if WatchDogMode:
gray = cv2.cvtColor(frame_image, cv2.COLOR_BGR2GRAY)
gray = cv2.GaussianBlur(gray, (21, 21), 0)
if avg is None:
print("[INFO] starting background model...")
avg = gray.copy().astype("float")
rawCapture.truncate(0)
continue
cv2.accumulateWeighted(gray, avg, 0.5)
frameDelta = cv2.absdiff(gray, cv2.convertScaleAbs(avg))
# threshold the delta image, dilate the thresholded image to fill
# in holes, then find contours on thresholded image
thresh = cv2.threshold(frameDelta, 5, 255,
cv2.THRESH_BINARY)[1]
thresh = cv2.dilate(thresh, None, iterations=2)
cnts = cv2.findContours(thresh.copy(), cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_SIMPLE)
cnts = imutils.grab_contours(cnts)
# print('x')
# loop over the contours
for c in cnts:
# if the contour is too small, ignore it
if cv2.contourArea(c) < 5000:
continue
# compute the bounding box for the contour, draw it on the frame,
# and update the text
(x, y, w, h) = cv2.boundingRect(c)
cv2.rectangle(frame_image, (x, y), (x + w, y + h),
(128, 255, 0), 1)
text = "Occupied"
motionCounter += 1
#print(motionCounter)
#print(text)
LED.colorWipe(255, 16, 0)
lastMovtionCaptured = timestamp
switch.switch(1, 1)
switch.switch(2, 1)
switch.switch(3, 1)
if (timestamp - lastMovtionCaptured).seconds >= 0.5:
LED.colorWipe(255, 255, 0)
switch.switch(1, 0)
switch.switch(2, 0)
switch.switch(3, 0)
if FindLineMode:
cvFindLine()
cv2.line(frame_image, (300, 240), (340, 240), (128, 255, 128), 1)
cv2.line(frame_image, (320, 220), (320, 260), (128, 255, 128), 1)
if FindLineMode and not frameRender:
encoded, buffer = cv2.imencode('.jpg', frame_findline)
else:
encoded, buffer = cv2.imencode('.jpg', frame_image)
jpg_as_text = base64.b64encode(buffer)
footage_socket.send(jpg_as_text)
rawCapture.truncate(0)
