def init():
""" Initiate and configure the variables needed for the 'setRobotMotorPower' command."""
# This function will assign the following global variables:
global FORWARD_POWER_RIGHT
global BACKWARD_POWER_RIGHT
global FORWARD_POWER_LEFT
global BACKWARD_POWER_LEFT
'''
GPIO.setwarnings(False)
# Motor Right
GPIO.setup(17, GPIO.OUT)
GPIO.setup(18, GPIO.OUT)
FORWARD_POWER_RIGHT = GPIO.PWM(18, 100)
BACKWARD_POWER_RIGHT = GPIO.PWM(17, 100)
FORWARD_POWER_RIGHT.start(0)
BACKWARD_POWER_RIGHT.start(0)
# Motor Left
GPIO.setup(22, GPIO.OUT)
GPIO.setup(23, GPIO.OUT)
FORWARD_POWER_LEFT = GPIO.PWM(23, 100)
BACKWARD_POWER_LEFT = GPIO.PWM(22, 100)
FORWARD_POWER_LEFT.start(0)
BACKWARD_POWER_LEFT.start(0)
'''
move.setup()
def __init__(self):
move.setup() # initialize motors
self.cmdvel_sub = rospy.Subscriber('/cmd_vel',
Twist,
self.cb,
queue_size=1)
def __init__(self):
self._lin_vel = 0
self._ang_vel = 0
self._cmd_lin_vel = 0
self._cmd_ang_vel = 0
self._wheel_vel = (0, 0)
self._axle_length = 0.155
self._wheel_radius = 0.070
self._left_motor_dir = 1
self._right_motor_dir = 0
self._line_pin_right = 35
self._line_pin_middle = 36
self._line_pin_left = 38
GPIO.setwarnings(False)
GPIO.setmode(GPIO.BOARD)
GPIO.setup(self._line_pin_right, GPIO.IN)
GPIO.setup(self._line_pin_middle, GPIO.IN)
GPIO.setup(self._line_pin_left, GPIO.IN)
move.setup()
move.motorStop()
self._twist_sub = rospy.Subscriber('cmd_vel',
Twist,
self._twist_callback,
queue_size=1)
self._sonar_pub = rospy.Publisher('sonar', Range, queue_size=1)
self._IR_pub = rospy.Publisher('ir', ArrayIR, queue_size=1)
def __init__(self):
self.frame_num = 0
self.fps = 0
self.stream_on = False
# orange-yellowish
self.colorUpper = (54, 255, 255)
self.colorLower = (24, 100, 100)
if FindColorMode:
move.setup()
def main():
print("Setup")
move.setup()
print("Running main")
move.move(
40, 'forward', 'no', 0
) # speed, {'forward','backward','no'}, {'left','right','no'}, 0 < radius <= 1
time.sleep(2)
move.motorStop()
time.sleep(2)
move.motor_left(1, move.Dir_forward,
40) # Non-zero to go, 0=forward 1=backwards, speed
time.sleep(2)
# move.motorStop()
move.motor_left(0, move.Dir_forward, 0)
time.sleep(2)
move.motor_right(1, 0, 40) # Non-zero to go, 0=forward 1=backwards, speed
time.sleep(2)
move.motorStop()
time.sleep(2)
print("Done")
async def recv_msg(websocket):
global speed_set, modeSelect
move.setup()
direction_command = 'no'
turn_command = 'no'
while True:
response = {
'status' : 'ok',
'title' : '',
'data' : None
}
data = ''
data = await websocket.recv()
try:
data = json.loads(data)
except Exception as e:
print('not A JSON')
if not data:
continue
if isinstance(data,str):
robotCtrl(data, response)
switchCtrl(data, response)
functionSelect(data, response)
configPWM(data, response)
if 'get_info' == data:
response['title'] = 'get_info'
response['data'] = [info.get_cpu_tempfunc(), info.get_cpu_use(), info.get_ram_info()]
if 'wsB' in data:
try:
set_B=data.split()
speed_set = int(set_B[1])
except:
pass
elif 'AR' == data:
modeSelect = 'AR'
screen.screen_show(4, 'ARM MODE ON')
try:
fpv.changeMode('ARM MODE ON')
except:
pass
elif 'PT' == data:
modeSelect = 'PT'
screen.screen_show(4, 'PT MODE ON')
try:
fpv.changeMode('PT MODE ON')
except:
pass
#CVFL
elif 'CVFL' == data:
flask_app.modeselect('findlineCV')
elif 'CVFLColorSet' in data:
color = int(data.split()[1])
flask_app.camera.colorSet(color)
elif 'CVFLL1' in data:
pos = int(data.split()[1])
flask_app.camera.linePosSet_1(pos)
elif 'CVFLL2' in data:
pos = int(data.split()[1])
flask_app.camera.linePosSet_2(pos)
elif 'CVFLSP' in data:
err = int(data.split()[1])
flask_app.camera.errorSet(err)
elif 'defEC' in data:#Z
fpv.defaultExpCom()
elif(isinstance(data,dict)):
if data['title'] == "findColorSet":
color = data['data']
flask_app.colorFindSet(color[0],color[1],color[2])
if not functionMode:
if OLED_connection:
screen.screen_show(5,'Functions OFF')
else:
pass
print(data)
response = json.dumps(response)
await websocket.send(response)
def setup():
move.setup()
def run():
global direction_command, turn_command, pos_input, catch_input, cir_input, ultrasonicMode, FindLineMode, FindColorMode
move.setup()
findline.setup()
info_threading=threading.Thread(target=info_send_client) #Define a thread for FPV and OpenCV
info_threading.setDaemon(True) #'True' means it is a front thread,it would close when the mainloop() closes
info_threading.start() #Thread starts
ultra_threading=threading.Thread(target=ultra_send_client) #Define a thread for FPV and OpenCV
ultra_threading.setDaemon(True) #'True' means it is a front thread,it would close when the mainloop() closes
ultra_threading.start() #Thread starts
findline_threading=threading.Thread(target=findline_thread) #Define a thread for FPV and OpenCV
findline_threading.setDaemon(True) #'True' means it is a front thread,it would close when the mainloop() closes
findline_threading.start() #Thread starts
#move.stand()
ws_R = 0
ws_G = 0
ws_B = 0
Y_pitch = 0
Y_pitch_MAX = 200
Y_pitch_MIN = -200
while True:
data = ''
data = str(tcpCliSock.recv(BUFSIZ).decode())
if not data:
continue
elif 'forward' == data:
direction_command = 'forward'
move.move(speed_set, direction_command, turn_command, rad)
elif 'backward' == data:
direction_command = 'backward'
move.move(speed_set, direction_command, turn_command, rad)
elif 'DS' in data:
direction_command = 'no'
move.move(speed_set, direction_command, turn_command, rad)
elif 'left' == data:
turn_command = 'left'
move.move(speed_set, direction_command, turn_command, rad)
elif 'right' == data:
turn_command = 'right'
move.move(speed_set, direction_command, turn_command, rad)
elif 'TS' in data:
turn_command = 'no'
move.move(speed_set, direction_command, turn_command, rad)
elif 'out' == data:
if pos_input < 17:
pos_input+=1
servo.hand_pos(pos_input)
elif 'in' == data:
if pos_input > 1:
pos_input-=1
servo.hand_pos(pos_input)
elif 'headup' == data:
servo.camera_ang('lookup',0)
elif 'headdown' == data:
servo.camera_ang('lookdown',0)
elif 'headhome' == data:
servo.initPosAll()
elif 'c_left' == data:
if cir_input < 12:
cir_input+=1
servo.cir_pos(cir_input)
elif 'c_right' == data:
if cir_input > 1:
cir_input-=1
servo.cir_pos(cir_input)
elif 'catch' == data:
if catch_input < 13:
catch_input+=1
servo.catch(catch_input)
elif 'loose' == data:
if catch_input > 1:
catch_input-=1
servo.catch(catch_input)
elif 'wsR' in data:
try:
set_R=data.split()
ws_R = int(set_R[1])
LED.colorWipe(ws_R,ws_G,ws_B)
except:
pass
elif 'wsG' in data:
try:
set_G=data.split()
ws_G = int(set_G[1])
LED.colorWipe(ws_R,ws_G,ws_B)
except:
pass
elif 'wsB' in data:
try:
set_B=data.split()
ws_B = int(set_B[1])
LED.colorWipe(ws_R,ws_G,ws_B)
except:
pass
elif 'FindColor' in data:
fpv.FindColor(1)
FindColorMode = 1
ultrasonicMode = 1
tcpCliSock.send(('FindColor').encode())
elif 'WatchDog' in data:
fpv.WatchDog(1)
tcpCliSock.send(('WatchDog').encode())
elif 'steady' in data:
ultrasonicMode = 1
tcpCliSock.send(('steady').encode())
elif 'FindLine' in data:
FindLineMode = 1
tcpCliSock.send(('FindLine').encode())
elif 'funEnd' in data:
fpv.FindColor(0)
fpv.WatchDog(0)
ultrasonicMode = 0
FindLineMode = 0
FindColorMode = 0
tcpCliSock.send(('FunEnd').encode())
move.motorStop()
time.sleep(0.3)
move.motorStop()
else:
pass
# File name : servo.py
# Description : Control Functions
# Author : William
# Date : 2020/03/17
import time
import RPi.GPIO as GPIO
import threading
from mpu6050 import mpu6050
import Adafruit_PCA9685
import os
import json
import ultra
import Kalman_filter
import move
move.setup()
kalman_filter_X = Kalman_filter.Kalman_filter(0.01,0.1)
pwm = Adafruit_PCA9685.PCA9685()
pwm.set_pwm_freq(50)
MPU_connection = 1
try:
sensor = mpu6050(0x68)
print('mpu6050 connected, PT MODE ON')
except:
MPU_connection = 0
print('mpu6050 disconnected, ARM MODE ON')
curpath = os.path.realpath(__file__)
def run():
global direction_command, turn_command, pos_input, catch_input, cir_input, ultrasonicMode, FindLineMode, FindColorMode
move.setup()
findline.setup()
info_threading = threading.Thread(
target=info_send_client) # Define a thread for FPV and OpenCV
info_threading.setDaemon(
True
) #'True' means it is a front thread,it would close when the mainloop() closes
info_threading.start() # Thread starts
ultra_threading = threading.Thread(
target=ultra_send_client) # Define a thread for FPV and OpenCV
ultra_threading.setDaemon(
True
) #'True' means it is a front thread,it would close when the mainloop() closes
ultra_threading.start() # Thread starts
findline_threading = threading.Thread(
target=findline_thread) # Define a thread for FPV and OpenCV
findline_threading.setDaemon(
True
) #'True' means it is a front thread,it would close when the mainloop() closes
findline_threading.start() # Thread starts
# move.stand()
ws_R = 0
ws_G = 0
ws_B = 0
Y_pitch = 0
Y_pitch_MAX = 200
Y_pitch_MIN = -200
while True:
data = ""
data = str(tcpCliSock.recv(BUFSIZ).decode())
if not data:
continue
elif "forward" == data:
direction_command = "forward"
move.move(speed_set, direction_command, turn_command, rad)
elif "backward" == data:
direction_command = "backward"
move.move(speed_set, direction_command, turn_command, rad)
elif "DS" in data:
direction_command = "no"
move.move(speed_set, direction_command, turn_command, rad)
elif "left" == data:
turn_command = "left"
move.move(speed_set, direction_command, turn_command, rad)
elif "right" == data:
turn_command = "right"
move.move(speed_set, direction_command, turn_command, rad)
elif "TS" in data:
turn_command = "no"
move.move(speed_set, direction_command, turn_command, rad)
elif "out" == data:
if pos_input < 17:
pos_input += 1
servo.hand_pos(pos_input)
elif "in" == data:
if pos_input > 1:
pos_input -= 1
servo.hand_pos(pos_input)
elif "headup" == data:
servo.camera_ang("lookup", 0)
elif "headdown" == data:
servo.camera_ang("lookdown", 0)
elif "headhome" == data:
servo.camera_ang("home", 0)
servo.hand("in")
servo.cir_pos(6)
pos_input = 1
catch_input = 1
cir_input = 6
servo.catch(catch_input)
time.sleep(0.5)
servo.clean_all()
elif "c_left" == data:
if cir_input < 12:
cir_input += 1
servo.cir_pos(cir_input)
elif "c_right" == data:
if cir_input > 1:
cir_input -= 1
servo.cir_pos(cir_input)
elif "catch" == data:
if catch_input < 13:
catch_input += 1
servo.catch(catch_input)
elif "loose" == data:
if catch_input > 1:
catch_input -= 1
servo.catch(catch_input)
elif "wsR" in data:
try:
set_R = data.split()
ws_R = int(set_R[1])
LED.colorWipe(Color(ws_R, ws_G, ws_B))
except:
pass
elif "wsG" in data:
try:
set_G = data.split()
ws_G = int(set_G[1])
LED.colorWipe(Color(ws_R, ws_G, ws_B))
except:
pass
elif "wsB" in data:
try:
set_B = data.split()
ws_B = int(set_B[1])
LED.colorWipe(Color(ws_R, ws_G, ws_B))
except:
pass
elif "FindColor" in data:
FindColorMode = 0
tcpCliSock.send(("FunEnd").encode())
elif "WatchDog" in data:
tcpCliSock.send(("FunEnd").encode())
elif "steady" in data:
ultrasonicMode = 1
tcpCliSock.send(("steady").encode())
elif "FindLine" in data:
FindLineMode = 1
tcpCliSock.send(("FindLine").encode())
elif "funEnd" in data:
ultrasonicMode = 0
FindLineMode = 0
FindColorMode = 0
tcpCliSock.send(("FunEnd").encode())
move.motorStop()
else:
pass
def run():
global servo_move, speed_set, servo_command, functionMode, init_get, R_set, G_set, B_set, SR_mode
servo.servo_init()
move.setup()
findline.setup()
direction_command = 'no'
turn_command = 'no'
servo_command = 'no'
speed_set = 100
rad = 0.5
info_threading = threading.Thread(
target=info_send_client) #Define a thread for FPV and OpenCV
info_threading.setDaemon(
True
) #'True' means it is a front thread,it would close when the mainloop() closes
info_threading.start() #Thread starts
servo_move = Servo_ctrl()
servo_move.start()
servo_move.pause()
findline.setup()
while True:
data = ''
data = str(tcpCliSock.recv(BUFSIZ).decode())
if not data:
continue
elif 'forward' == data:
direction_command = 'forward'
move.move(speed_set, direction_command)
elif 'backward' == data:
direction_command = 'backward'
move.move(speed_set, direction_command)
elif 'DS' in data:
direction_command = 'no'
move.move(speed_set, direction_command)
elif 'left' == data:
# turn_command = 'left'
servo.turnLeft()
elif 'right' == data:
# turn_command = 'right'
servo.turnRight()
elif 'TS' in data:
# turn_command = 'no'
servo.turnMiddle()
elif 'Switch_1_on' in data:
fpv.Sounds(1)
tcpCliSock.send(('Switch_1_on').encode())
elif 'Switch_1_off' in data:
fpv.Sounds(0)
tcpCliSock.send(('Switch_1_off').encode())
elif 'Switch_2_on' in data:
#switch.switch(2,1)
fpv.Radar(1)
tcpCliSock.send(('Switch_2_on').encode())
elif 'Switch_2_off' in data:
#switch.switch(2,0)
fpv.Radar(0)
tcpCliSock.send(('Switch_2_off').encode())
elif 'Switch_3_on' in data:
switch.switch(3, 1)
tcpCliSock.send(('Switch_3_on').encode())
elif 'Switch_3_off' in data:
switch.switch(3, 0)
tcpCliSock.send(('Switch_3_off').encode())
elif 'Switch_A_on' in data:
tcpCliSock.send(('Switch_A_on').encode())
elif 'Switch_A_off' in data:
tcpCliSock.send(('Switch_A_off').encode())
elif 'Switch_B_on' in data:
tcpCliSock.send(('Switch_B_on').encode())
elif 'Switch_B_off' in data:
tcpCliSock.send(('Switch_B_off').encode())
elif 'Switch_C_on' in data:
fpv.HaarJJ(1)
tcpCliSock.send(('Switch_C_on').encode())
elif 'Switch_C_off' in data:
fpv.HaarJJ(0)
tcpCliSock.send(('Switch_C_off').encode())
elif 'Face_Track_on' in data:
# functionMode = 3
fpv.FaceTrack(1)
tcpCliSock.send(('Face_Track_on').encode())
elif 'Face_Track_off' in data:
# functionMode = 0
fpv.FaceTrack(0)
tcpCliSock.send(('Face_Track_off').encode())
elif 'function_1_on' in data:
servo.ahead()
time.sleep(0.2)
tcpCliSock.send(('function_1_on').encode())
radar_send = servo.radar_scan()
tcpCliSock.sendall(radar_send.encode())
print(radar_send)
time.sleep(0.3)
tcpCliSock.send(('function_1_off').encode())
elif 'function_2_on' in data:
functionMode = 2
fpv.FindColor(1)
tcpCliSock.send(('function_2_on').encode())
elif 'function_2_off' in data:
functionMode = 0
fpv.FindColor(0)
switch.switch(1, 0)
switch.switch(2, 0)
switch.switch(3, 0)
tcpCliSock.send(('function_2_off').encode())
elif 'function_3_on' in data:
functionMode = 3
fpv.WatchDog(1)
tcpCliSock.send(('function_3_on').encode())
elif 'function_3_off' in data:
functionMode = 0
fpv.WatchDog(0)
tcpCliSock.send(('function_3_off').encode())
elif 'function_4_on' in data:
functionMode = 4
servo_move.resume()
tcpCliSock.send(('function_4_on').encode())
elif 'function_4_off' in data:
functionMode = 0
servo_move.pause()
move.motorStop()
tcpCliSock.send(('function_4_off').encode())
elif 'function_5_on' in data:
functionMode = 5
servo_move.resume()
tcpCliSock.send(('function_5_on').encode())
elif 'function_5_off' in data:
functionMode = 0
servo_move.pause()
move.motorStop()
tcpCliSock.send(('function_5_off').encode())
elif 'function_6_on' in data:
if MPU_connection:
functionMode = 6
servo_move.resume()
tcpCliSock.send(('function_6_on').encode())
elif 'function_6_off' in data:
functionMode = 0
servo_move.pause()
move.motorStop()
init_get = 0
tcpCliSock.send(('function_6_off').encode())
#elif 'function_1_off' in data:
# tcpCliSock.send(('function_1_off').encode())
elif 'lookleft' == data:
servo_command = 'lookleft'
servo_move.resume()
elif 'lookright' == data:
servo_command = 'lookright'
servo_move.resume()
elif 'up' == data:
servo_command = 'up'
servo_move.resume()
elif 'down' == data:
servo_command = 'down'
servo_move.resume()
elif 'stop' == data:
if not functionMode:
servo_move.pause()
servo_command = 'no'
pass
elif 'home' == data:
servo.ahead()
elif 'CVrun' == data:
if not FPV.CVrun:
FPV.CVrun = 1
tcpCliSock.send(('CVrun_on').encode())
else:
FPV.CVrun = 0
tcpCliSock.send(('CVrun_off').encode())
elif 'wsR' in data:
try:
set_R = data.split()
R_set = int(set_R[1])
led.colorWipe(R_set, G_set, B_set)
except:
pass
elif 'wsG' in data:
try:
set_G = data.split()
G_set = int(set_G[1])
led.colorWipe(R_set, G_set, B_set)
except:
pass
elif 'wsB' in data:
try:
set_B = data.split()
B_set = int(set_B[1])
led.colorWipe(R_set, G_set, B_set)
except:
pass
elif 'pwm0' in data:
try:
set_pwm0 = data.split()
pwm0_set = int(set_pwm0[1])
servo.setPWM(0, pwm0_set)
except:
pass
elif 'pwm1' in data:
try:
set_pwm1 = data.split()
pwm1_set = int(set_pwm1[1])
servo.setPWM(1, pwm1_set)
except:
pass
elif 'pwm2' in data:
try:
set_pwm2 = data.split()
pwm2_set = int(set_pwm2[1])
servo.setPWM(2, pwm2_set)
except:
pass
elif 'Speed' in data:
try:
set_speed = data.split()
speed_set = int(set_speed[1])
except:
pass
elif 'Save' in data:
try:
servo.saveConfig()
except:
pass
elif 'CVFL' in data:
if not FPV.FindLineMode:
FPV.FindLineMode = 1
tcpCliSock.send(('CVFL_on').encode())
else:
move.motorStop()
FPV.FindLineMode = 0
tcpCliSock.send(('CVFL_off').encode())
elif 'Render' in data:
if FPV.frameRender:
FPV.frameRender = 0
else:
FPV.frameRender = 1
elif 'WBswitch' in data:
if FPV.lineColorSet == 255:
FPV.lineColorSet = 0
else:
FPV.lineColorSet = 255
elif 'lip1' in data:
try:
set_lip1 = data.split()
lip1_set = int(set_lip1[1])
FPV.linePos_1 = lip1_set
except:
pass
elif 'lip2' in data:
try:
set_lip2 = data.split()
lip2_set = int(set_lip2[1])
FPV.linePos_2 = lip2_set
except:
pass
elif 'err' in data:
try:
set_err = data.split()
err_set = int(set_err[1])
FPV.findLineError = err_set
except:
pass
elif 'FCSET' in data:
FCSET = data.split()
fpv.colorFindSet(int(FCSET[1]), int(FCSET[2]), int(FCSET[3]))
elif 'setEC' in data: #Z
ECset = data.split()
try:
fpv.setExpCom(int(ECset[1]))
except:
pass
elif 'defEC' in data: #Z
fpv.defaultExpCom()
elif 'police' in data:
if LED.ledfunc != 'police':
tcpCliSock.send(('rainbow_off').encode())
LED.ledfunc = 'police'
ledthread.resume()
tcpCliSock.send(('police_on').encode())
elif LED.ledfunc == 'police':
LED.ledfunc = ''
ledthread.pause()
tcpCliSock.send(('police_off').encode())
elif 'rainbow' in data:
if LED.ledfunc != 'rainbow':
tcpCliSock.send(('police_off').encode())
LED.ledfunc = 'rainbow'
ledthread.resume()
tcpCliSock.send(('rainbow_on').encode())
elif LED.ledfunc == 'rainbow':
LED.ledfunc = ''
ledthread.pause()
tcpCliSock.send(('rainbow_off').encode())
elif 'sr' in data:
if not SR_mode:
if SR_dect:
SR_mode = 1
tcpCliSock.send(('sr_on').encode())
sr.resume()
elif SR_mode:
SR_mode = 0
sr.pause()
move.motorStop()
tcpCliSock.send(('sr_off').encode())
else:
pass
print(data)
class CVThread(threading.Thread):
font = cv2.FONT_HERSHEY_SIMPLEX
kalman_filter_X = Kalman_filter.Kalman_filter(0.01, 0.1)
kalman_filter_Y = Kalman_filter.Kalman_filter(0.01, 0.1)
P_direction = 1
T_direction = -1
P_servo = 0
T_servo = 1
P_anglePos = 0
T_anglePos = 0
cameraDiagonalW = 64
cameraDiagonalH = 48
videoW = 640
videoH = 480
Y_lock = 0
X_lock = 0
tor = 17
scGear = RPIservo.ServoCtrl()
scGear.moveInit()
move.setup()
switch.switchSetup()
def __init__(self, *args, **kwargs):
self.CVThreading = 0
self.CVMode = 'none'
self.imgCV = None
self.mov_x = None
self.mov_y = None
self.mov_w = None
self.mov_h = None
self.radius = 0
self.box_x = None
self.box_y = None
self.drawing = 0
self.findColorDetection = 0
self.left_Pos1 = None
self.right_Pos1 = None
self.center_Pos1 = None
self.left_Pos2 = None
self.right_Pos2 = None
self.center_Pos2 = None
self.center = None
super(CVThread, self).__init__(*args, **kwargs)
self.__flag = threading.Event()
self.__flag.clear()
self.avg = None
self.motionCounter = 0
self.lastMovtionCaptured = datetime.datetime.now()
self.frameDelta = None
self.thresh = None
self.cnts = None
def mode(self, invar, imgInput):
self.CVMode = invar
self.imgCV = imgInput
self.resume()
def elementDraw(self, imgInput):
if self.CVMode == 'none':
pass
elif self.CVMode == 'findColor':
if self.findColorDetection:
cv2.putText(imgInput, 'Target Detected', (40, 60),
CVThread.font, 0.5, (255, 255, 255), 1,
cv2.LINE_AA)
self.drawing = 1
else:
cv2.putText(imgInput, 'Target Detecting', (40, 60),
CVThread.font, 0.5, (255, 255, 255), 1,
cv2.LINE_AA)
self.drawing = 0
if self.radius > 10 and self.drawing:
cv2.rectangle(imgInput, (int(self.box_x - self.radius),
int(self.box_y + self.radius)),
(int(self.box_x + self.radius),
int(self.box_y - self.radius)), (255, 255, 255),
1)
elif self.CVMode == 'findlineCV':
if frameRender:
imgInput = cv2.cvtColor(imgInput, cv2.COLOR_BGR2GRAY)
retval_bw, imgInput = cv2.threshold(imgInput, 0, 255,
cv2.THRESH_OTSU)
imgInput = cv2.erode(imgInput, None, iterations=6)
try:
if lineColorSet == 255:
cv2.putText(imgInput, ('Following White Line'), (30, 50),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (128, 255, 128),
1, cv2.LINE_AA)
else:
cv2.putText(imgInput, ('Following Black Line'), (30, 50),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (128, 255, 128),
1, cv2.LINE_AA)
cv2.line(imgInput, (self.left_Pos1, (linePos_1 + 30)),
(self.left_Pos1, (linePos_1 - 30)), (255, 128, 64), 1)
cv2.line(
imgInput,
(self.right_Pos1, (linePos_1 + 30)),
(self.right_Pos1, (linePos_1 - 30)),
(64, 128, 255),
)
cv2.line(imgInput, (0, linePos_1), (640, linePos_1),
(255, 255, 64), 1)
cv2.line(imgInput, (self.left_Pos2, (linePos_2 + 30)),
(self.left_Pos2, (linePos_2 - 30)), (255, 128, 64), 1)
cv2.line(imgInput, (self.right_Pos2, (linePos_2 + 30)),
(self.right_Pos2, (linePos_2 - 30)), (64, 128, 255),
1)
cv2.line(imgInput, (0, linePos_2), (640, linePos_2),
(255, 255, 64), 1)
cv2.line(imgInput,
((self.center - 20), int(
(linePos_1 + linePos_2) / 2)),
((self.center + 20), int(
(linePos_1 + linePos_2) / 2)), (0, 0, 0), 1)
cv2.line(
imgInput,
((self.center), int((linePos_1 + linePos_2) / 2 + 20)),
((self.center), int((linePos_1 + linePos_2) / 2 - 20)),
(0, 0, 0), 1)
except:
pass
elif self.CVMode == 'watchDog':
if self.drawing:
cv2.rectangle(
imgInput, (self.mov_x, self.mov_y),
(self.mov_x + self.mov_w, self.mov_y + self.mov_h),
(128, 255, 0), 1)
return imgInput
def watchDog(self, imgInput):
timestamp = datetime.datetime.now()
gray = cv2.cvtColor(imgInput, cv2.COLOR_BGR2GRAY)
gray = cv2.GaussianBlur(gray, (21, 21), 0)
if self.avg is None:
print("[INFO] starting background model...")
self.avg = gray.copy().astype("float")
return 'background model'
cv2.accumulateWeighted(gray, self.avg, 0.5)
self.frameDelta = cv2.absdiff(gray, cv2.convertScaleAbs(self.avg))
# threshold the delta image, dilate the thresholded image to fill
# in holes, then find contours on thresholded image
self.thresh = cv2.threshold(self.frameDelta, 5, 255,
cv2.THRESH_BINARY)[1]
self.thresh = cv2.dilate(self.thresh, None, iterations=2)
self.cnts = cv2.findContours(self.thresh.copy(), cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_SIMPLE)
self.cnts = imutils.grab_contours(self.cnts)
# print('x')
# loop over the contours
for c in self.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
(self.mov_x, self.mov_y, self.mov_w,
self.mov_h) = cv2.boundingRect(c)
self.drawing = 1
self.motionCounter += 1
#print(motionCounter)
#print(text)
self.lastMovtionCaptured = timestamp
switch.switch(1, 1)
switch.switch(2, 1)
switch.switch(3, 1)
light.setColor(255, 64, 0)
if (timestamp - self.lastMovtionCaptured).seconds >= 0.5:
self.drawing = 0
switch.switch(1, 0)
switch.switch(2, 0)
switch.switch(3, 0)
light.setColor(0, 64, 255)
self.pause()
def findLineCtrl(self, posInput, setCenter): #2
if posInput and setCenter:
print(posInput, setCenter)
if posInput > (setCenter + findLineError):
if CVRun:
move.move(80, 'no', 'right', 0.5)
else:
move.move(80, 'no', 'no', 0.5)
pass
elif posInput < (setCenter - findLineError):
if CVRun:
move.move(80, 'no', 'left', 0.5)
else:
move.move(80, 'no', 'no', 0.5)
pass
else:
if CVRun:
move.move(80, 'forward', 'no', 0.5)
else:
move.move(80, 'no', 'no', 0.5)
pass
else:
move.motorStop()
def findlineCV(self, frame_image):
frame_findline = cv2.cvtColor(frame_image, cv2.COLOR_BGR2GRAY)
retval, frame_findline = cv2.threshold(frame_findline, 0, 255,
cv2.THRESH_OTSU)
frame_findline = cv2.erode(frame_findline, None, iterations=6)
colorPos_1 = frame_findline[linePos_1]
colorPos_2 = frame_findline[linePos_2]
try:
lineColorCount_Pos1 = np.sum(colorPos_1 == lineColorSet)
lineColorCount_Pos2 = np.sum(colorPos_2 == lineColorSet)
lineIndex_Pos1 = np.where(colorPos_1 == lineColorSet)
lineIndex_Pos2 = np.where(colorPos_2 == lineColorSet)
if lineColorCount_Pos1 == 0:
lineColorCount_Pos1 = 1
if lineColorCount_Pos2 == 0:
lineColorCount_Pos2 = 1
self.left_Pos1 = lineIndex_Pos1[0][lineColorCount_Pos1 - 1]
self.right_Pos1 = lineIndex_Pos1[0][0]
self.center_Pos1 = int((self.left_Pos1 + self.right_Pos1) / 2)
self.left_Pos2 = lineIndex_Pos2[0][lineColorCount_Pos2 - 1]
self.right_Pos2 = lineIndex_Pos2[0][0]
self.center_Pos2 = int((self.left_Pos2 + self.right_Pos2) / 2)
self.center = int((self.center_Pos1 + self.center_Pos2) / 2)
except:
center = None
move.motorStop()
pass
self.findLineCtrl(self.center, 320)
self.pause()
def servoMove(ID, Dir, errorInput):
if ID == CVThread.P_servo:
errorGenOut = CVThread.kalman_filter_X.kalman(errorInput)
CVThread.P_anglePos += 0.15 * (
errorGenOut * Dir) * CVThread.cameraDiagonalW / CVThread.videoW
if abs(errorInput) > CVThread.tor:
CVThread.scGear.moveAngle(ID, CVThread.P_anglePos)
CVThread.X_lock = 0
else:
CVThread.X_lock = 1
elif ID == CVThread.T_servo:
errorGenOut = CVThread.kalman_filter_Y.kalman(errorInput)
CVThread.T_anglePos += 0.15 * (
errorGenOut * Dir) * CVThread.cameraDiagonalH / CVThread.videoH
if abs(errorInput) > CVThread.tor:
CVThread.scGear.moveAngle(ID, CVThread.T_anglePos)
CVThread.Y_lock = 0
else:
CVThread.Y_lock = 1
else:
print('No servoPort %d assigned.' % ID)
def findColor(self, frame_image):
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:
self.findColorDetection = 1
c = max(cnts, key=cv2.contourArea)
((self.box_x, self.box_y), self.radius) = cv2.minEnclosingCircle(c)
M = cv2.moments(c)
center = (int(M["m10"] / M["m00"]), int(M["m01"] / M["m00"]))
X = int(self.box_x)
Y = int(self.box_y)
error_Y = 240 - Y
error_X = 320 - X
CVThread.servoMove(CVThread.P_servo, CVThread.P_direction, error_X)
CVThread.servoMove(CVThread.T_servo, CVThread.T_direction, error_Y)
if CVThread.X_lock == 1 and CVThread.Y_lock == 1:
switch.switch(1, 1)
switch.switch(2, 1)
switch.switch(3, 1)
light.setColor(255, 64, 0)
else:
switch.switch(1, 0)
switch.switch(2, 0)
switch.switch(3, 0)
light.setColor(0, 64, 255)
else:
self.findColorDetection = 0
move.motorStop()
self.pause()
def pause(self):
self.__flag.clear()
def resume(self):
self.__flag.set()
def run(self):
while 1:
self.__flag.wait()
if self.CVMode == 'none':
continue
elif self.CVMode == 'findColor':
self.CVThreading = 1
self.findColor(self.imgCV)
self.CVThreading = 0
elif self.CVMode == 'findlineCV':
self.CVThreading = 1
self.findlineCV(self.imgCV)
self.CVThreading = 0
elif self.CVMode == 'watchDog':
self.CVThreading = 1
self.watchDog(self.imgCV)
self.CVThreading = 0
pass
#!/usr/bin/python3 import Adafruit_PCA9685 import numpy as np import time import threading import oledCtrl import robotLight import move as dc dc.setup() dc.motorStop() from mpu6050 import mpu6050 import Kalman_filter light = robotLight.RobotLight() light.start() pwm = Adafruit_PCA9685.PCA9685() pwm.set_pwm_freq(50) ''' 0 autoSelect 1 diagonalSelect 2 triangularSelect ''' selectGait = 0 speedApart = 50 init_pwm0 = 348 init_pwm1 = 215
def run():
global direction_command, turn_command, pos_input, catch_input, cir_input, ultrasonicMode, FindLineMode, FindColorMode, SportModeOn
move.setup()
findline.setup()
info_threading = threading.Thread(
target=info_send_client) #Define a thread for FPV and OpenCV
info_threading.setDaemon(
True
) #'True' means it is a front thread,it would close when the mainloop() closes
info_threading.start() #Thread starts
ultra_threading = threading.Thread(
target=ultra_send_client) #Define a thread for FPV and OpenCV
ultra_threading.setDaemon(
True
) #'True' means it is a front thread,it would close when the mainloop() closes
ultra_threading.start() #Thread starts
findline_threading = threading.Thread(
target=findline_thread) #Define a thread for FPV and OpenCV
findline_threading.setDaemon(
True
) #'True' means it is a front thread,it would close when the mainloop() closes
findline_threading.start() #Thread starts
#move.stand()
ws_R = 0
ws_G = 0
ws_B = 0
Y_pitch = 0
Y_pitch_MAX = 200
Y_pitch_MIN = -200
while True:
data = ''
data = str(tcpCliSock.recv(BUFSIZ).decode())
if not data:
continue
elif 'SportModeOn' == data:
SportModeOn = 1
tcpCliSock.send(('SportModeOn').encode())
elif 'SportModeOff' == data:
SportModeOn = 0
tcpCliSock.send(('SportModeOff').encode())
elif 'forward' == data:
direction_command = 'forward'
if SportModeOn:
move.move(speed_set, direction_command, turn_command, rad)
else:
move.move(SpeedBase, direction_command, turn_command, rad)
elif 'backward' == data:
direction_command = 'backward'
if SportModeOn:
move.move(speed_set, direction_command, turn_command, rad)
else:
move.move(SpeedBase, direction_command, turn_command, rad)
elif 'DS' in data:
direction_command = 'no'
if SportModeOn:
move.move(speed_set, direction_command, turn_command, rad)
else:
move.move(SpeedBase, direction_command, turn_command, rad)
elif 'left' == data:
turn_command = 'left'
if SportModeOn:
move.move(speed_set, direction_command, turn_command, rad)
else:
move.move(SpeedBase, direction_command, turn_command, rad)
elif 'right' == data:
turn_command = 'right'
if SportModeOn:
move.move(speed_set, direction_command, turn_command, rad)
else:
move.move(SpeedBase, direction_command, turn_command, rad)
elif 'TS' in data:
turn_command = 'no'
move.move(speed_set, direction_command, turn_command, rad)
elif 'headup' == data:
servo.camera_ang('lookup', 'no')
elif 'headdown' == data:
servo.camera_ang('lookdown', 'no')
elif 'headhome' == data:
servo.camera_ang('home', 'no')
time.sleep(0.2)
servo.clean_all()
elif 'wsR' in data:
try:
set_R = data.split()
ws_R = int(set_R[1])
LED.colorWipe(Color(ws_R, ws_G, ws_B))
except:
pass
elif 'wsG' in data:
try:
set_G = data.split()
ws_G = int(set_G[1])
LED.colorWipe(Color(ws_R, ws_G, ws_B))
except:
pass
elif 'wsB' in data:
try:
set_B = data.split()
ws_B = int(set_B[1])
LED.colorWipe(Color(ws_R, ws_G, ws_B))
except:
pass
elif 'FindColor' in data:
FindColorMode = 0
tcpCliSock.send(('FunEnd').encode())
elif 'WatchDog' in data:
tcpCliSock.send(('FunEnd').encode())
elif 'steady' in data:
ultrasonicMode = 1
tcpCliSock.send(('steady').encode())
elif 'FindLine' in data:
FindLineMode = 1
tcpCliSock.send(('FindLine').encode())
elif 'funEnd' in data:
ultrasonicMode = 0
FindLineMode = 0
FindColorMode = 0
tcpCliSock.send(('FunEnd').encode())
move.motorStop()
else:
pass
def run():
global servo_move, speed_set, servo_command, functionMode, init_get
servo.servo_init()
move.setup()
findline.setup()
direction_command = 'no'
turn_command = 'no'
servo_command = 'no'
speed_set = 100
rad = 0.5
info_threading = threading.Thread(
target=info_send_client) #Define a thread for FPV and OpenCV
info_threading.setDaemon(
True
) #'True' means it is a front thread,it would close when the mainloop() closes
info_threading.start() #Thread starts
servo_move = Servo_ctrl()
servo_move.start()
servo_move.pause()
findline.setup()
while True:
data = ''
data = str(tcpCliSock.recv(BUFSIZ).decode())
if not data:
continue
elif 'forward' == data:
direction_command = 'forward'
move.move(speed_set, direction_command, turn_command, rad)
elif 'backward' == data:
direction_command = 'backward'
move.move(speed_set, direction_command, turn_command, rad)
elif 'DS' in data:
direction_command = 'no'
move.move(speed_set, direction_command, turn_command, rad)
elif 'left' == data:
turn_command = 'left'
move.move(speed_set, direction_command, turn_command, rad)
elif 'right' == data:
turn_command = 'right'
move.move(speed_set, direction_command, turn_command, rad)
elif 'TS' in data:
turn_command = 'no'
move.move(speed_set, direction_command, turn_command, rad)
elif 'Switch_1_on' in data:
switch.switch(1, 1)
tcpCliSock.send(('Switch_1_on').encode())
elif 'Switch_1_off' in data:
switch.switch(1, 0)
tcpCliSock.send(('Switch_1_off').encode())
elif 'Switch_2_on' in data:
switch.switch(2, 1)
tcpCliSock.send(('Switch_2_on').encode())
elif 'Switch_2_off' in data:
switch.switch(2, 0)
tcpCliSock.send(('Switch_2_off').encode())
elif 'Switch_3_on' in data:
switch.switch(3, 1)
tcpCliSock.send(('Switch_3_on').encode())
elif 'Switch_3_off' in data:
switch.switch(3, 0)
tcpCliSock.send(('Switch_3_off').encode())
elif 'function_1_on' in data:
servo.ahead()
time.sleep(0.2)
tcpCliSock.send(('function_1_on').encode())
radar_send = servo.radar_scan()
tcpCliSock.sendall(radar_send.encode())
print(radar_send)
time.sleep(0.3)
tcpCliSock.send(('function_1_off').encode())
elif 'function_2_on' in data:
functionMode = 2
fpv.FindColor(1)
tcpCliSock.send(('function_2_on').encode())
elif 'function_3_on' in data:
functionMode = 3
fpv.WatchDog(1)
tcpCliSock.send(('function_3_on').encode())
elif 'function_4_on' in data:
functionMode = 4
servo_move.resume()
tcpCliSock.send(('function_4_on').encode())
elif 'function_5_on' in data:
functionMode = 5
servo_move.resume()
tcpCliSock.send(('function_5_on').encode())
elif 'function_6_on' in data:
if MPU_connection:
functionMode = 6
servo_move.resume()
tcpCliSock.send(('function_6_on').encode())
#elif 'function_1_off' in data:
# tcpCliSock.send(('function_1_off').encode())
elif 'function_2_off' in data:
functionMode = 0
fpv.FindColor(0)
switch.switch(1, 0)
switch.switch(2, 0)
switch.switch(3, 0)
tcpCliSock.send(('function_2_off').encode())
elif 'function_3_off' in data:
functionMode = 0
fpv.WatchDog(0)
tcpCliSock.send(('function_3_off').encode())
elif 'function_4_off' in data:
functionMode = 0
servo_move.pause()
move.motorStop()
tcpCliSock.send(('function_4_off').encode())
elif 'function_5_off' in data:
functionMode = 0
servo_move.pause()
move.motorStop()
tcpCliSock.send(('function_5_off').encode())
elif 'function_6_off' in data:
functionMode = 0
servo_move.pause()
move.motorStop()
init_get = 0
tcpCliSock.send(('function_6_off').encode())
elif 'lookleft' == data:
servo_command = 'lookleft'
servo_move.resume()
elif 'lookright' == data:
servo_command = 'lookright'
servo_move.resume()
elif 'up' == data:
servo_command = 'up'
servo_move.resume()
elif 'down' == data:
servo_command = 'down'
servo_move.resume()
elif 'lookup' == data:
servo_command = 'lookup'
servo_move.resume()
elif 'lookdown' == data:
servo_command = 'lookdown'
servo_move.resume()
elif 'grab' == data:
servo_command = 'grab'
servo_move.resume()
elif 'loose' == data:
servo_command = 'loose'
servo_move.resume()
elif 'stop' == data:
if not functionMode:
servo_move.pause()
servo_command = 'no'
pass
elif 'home' == data:
servo.ahead()
elif 'wsB' in data:
try:
set_B = data.split()
speed_set = int(set_B[1])
except:
pass
elif 'CVFL' in data:
if not FPV.FindLineMode:
FPV.FindLineMode = 1
tcpCliSock.send(('CVFL_on').encode())
else:
move.motorStop()
FPV.FindLineMode = 0
tcpCliSock.send(('CVFL_off').encode())
elif 'Render' in data:
if FPV.frameRender:
FPV.frameRender = 0
else:
FPV.frameRender = 1
elif 'WBswitch' in data:
if FPV.lineColorSet == 255:
FPV.lineColorSet = 0
else:
FPV.lineColorSet = 255
elif 'lip1' in data:
try:
set_lip1 = data.split()
lip1_set = int(set_lip1[1])
FPV.linePos_1 = lip1_set
except:
pass
elif 'lip2' in data:
try:
set_lip2 = data.split()
lip2_set = int(set_lip2[1])
FPV.linePos_2 = lip2_set
except:
pass
elif 'err' in data:
try:
set_err = data.split()
err_set = int(set_err[1])
FPV.findLineError = err_set
except:
pass
elif 'FCSET' in data:
FCSET = data.split()
fpv.colorFindSet(int(FCSET[1]), int(FCSET[2]), int(FCSET[3]))
elif 'setEC' in data: #Z
ECset = data.split()
try:
fpv.setExpCom(int(ECset[1]))
except:
pass
elif 'defEC' in data: #Z
fpv.defaultExpCom()
else:
pass
print(data)
def capture_thread(self,IPinver):
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
#time.sleep(4)
lastMovtionCaptured = datetime.datetime.now()
time.sleep(0.1)
for frame in camera.capture_continuous(rawCapture, format="bgr", use_video_port=True):
frame_image = frame.array
cv2.line(frame_image,(300,240),(340,240),(128,255,128),1)
cv2.line(frame_image,(320,220),(320,260),(128,255,128),1)
timestamp = datetime.datetime.now()
mask = []
if FindColorMode:
move.setup()
####>>>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', 'right', 0.6)
#print('turn right')
#time.sleep(0.1)
#move.motorStop()
X_lock = 0
elif X > (330+tor*3):
move.move(70, 'no', 'left', 0.6)
#print('turn left')
#time.sleep(0.1)
#move.motorStop()
X_lock = 0
else:
move.motorStop()
X_lock = 1
if X_lock == 1 and Y_lock == 1:
if 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()
cv2.imshow("Frame", frame_image) # Display image
cv2.imshow("Mask", mask)
key = cv2.waitKey(1) & 0xFF
# time.sleep(1)
rawCapture.truncate(0)
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<<<####
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(Color(255,16,0))
lastMovtionCaptured = timestamp
if (timestamp - lastMovtionCaptured).seconds >= 0.5:
LED.colorWipe(Color(255,255,0))
encoded, buffer = cv2.imencode('.jpg', frame_image)
jpg_as_text = base64.b64encode(buffer)
footage_socket.send(jpg_as_text)
rawCapture.truncate(0)
