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)
footage_socket.set_hwm(1)
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
print("XYLock: X: " + str(X_lock) + " | Y: " + str(Y_lock))
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 FaceTrackMode:
#runFaceClassifer(frame_image)
runObjectClassifier(frame_image)
elif HaarJJMode:
import custom_haar_detector
if SoundsMode:
frame_image = custom_haar_detector.run_detector(
frame_image, play_sound=True, sound_min_gap_secs=10)
else:
frame_image = custom_haar_detector.run_detector(
frame_image)
if RadarMode:
dist = ultra.checkdist()
cv2.putText(frame_image, 'Radar: ' + str(dist), (40, 410),
font, 0.5, (255, 255, 255), 1, cv2.LINE_AA)
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)
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()
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()
if FindColorMode:
####>>>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:
if UltraData > 0.5:
LED.colorWipe(Color(255,16,0))
move.move(70, 'forward', 'no', 0.6)
elif UltraData < 0.4:
LED.colorWipe(Color(0,16,255))
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<<<####
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(0,16,255))
encoded, buffer = cv2.imencode('.jpg', frame_image)
jpg_as_text = base64.b64encode(buffer)
footage_socket.send(jpg_as_text)
rawCapture.truncate(0)
def run(self):
global goal_pos, servo_command, init_get, functionMode
while self.__running.isSet():
self.__flag.wait() # 为True时立即返回, 为False时阻塞直到内部的标识位为True后返回
if functionMode != 6:
if servo_command == 'lookleft':
servo.lookleft(servo_speed)
elif servo_command == 'lookright':
servo.lookright(servo_speed)
elif servo_command == 'up':
servo.up(servo_speed)
elif servo_command == 'down':
servo.down(servo_speed)
elif servo_command == 'lookup':
servo.lookup(servo_speed)
elif servo_command == 'lookdown':
servo.lookdown(servo_speed)
elif servo_command == 'grab':
servo.grab(servo_speed)
elif servo_command == 'loose':
servo.loose(servo_speed)
else:
pass
if functionMode == 4:
servo.ahead()
findline.run()
if not functionMode:
move.motorStop()
elif functionMode == 5:
servo.ahead()
dis_get = ultra.checkdist()
if dis_get < 0.15:
move.motorStop()
move.move(100, 'backward', 'no', 1)
move.motorStop()
move.move(100, 'no', 'left', 1)
time.sleep(0.2)
move.motorStop()
else:
move.move(100, 'forward', 'no', 1)
if not functionMode:
move.motorStop()
elif functionMode == 6:
if MPU_connection:
accelerometer_data = sensor.get_accel_data()
Y_get = accelerometer_data['y']
if not init_get:
goal_pos = Y_get
init_get = 1
if servo_command == 'up':
servo.up(servo_speed)
time.sleep(0.2)
accelerometer_data = sensor.get_accel_data()
Y_get = accelerometer_data['y']
goal_pos = Y_get
elif servo_command == 'down':
servo.down(servo_speed)
time.sleep(0.2)
accelerometer_data = sensor.get_accel_data()
Y_get = accelerometer_data['y']
goal_pos = Y_get
if abs(Y_get - goal_pos) > tor_pos:
if Y_get < goal_pos:
servo.down(int(mpu_speed * abs(Y_get - goal_pos)))
elif Y_get > goal_pos:
servo.up(int(mpu_speed * abs(Y_get - goal_pos)))
time.sleep(0.03)
continue
else:
functionMode = 0
try:
self.pause()
except:
pass
time.sleep(0.07)
def robotCtrl(command_input, response):
global direction_command, turn_command
if 'forward' == command_input:
direction_command = 'forward'
move.motor_left(1, 0, speed_set)
move.motor_right(1, 0, speed_set)
RL.both_on()
elif 'backward' == command_input:
direction_command = 'backward'
move.motor_left(1, 1, speed_set)
move.motor_right(1, 1, speed_set)
RL.red()
elif 'DS' in command_input:
direction_command = 'no'
move.motorStop()
if turn_command == 'left':
RL.both_off()
RL.turnLeft()
elif turn_command == 'right':
RL.both_off()
RL.turnRight()
elif turn_command == 'no':
RL.both_off()
elif 'left' == command_input:
turn_command = 'left'
scGear.moveAngle(2, 45)
RL.both_off()
RL.turnLeft()
elif 'right' == command_input:
turn_command = 'right'
scGear.moveAngle(2, -45)
RL.both_off()
RL.turnRight()
elif 'TS' in command_input:
turn_command = 'no'
scGear.moveAngle(2, 0)
if direction_command == 'forward':
RL.both_on()
elif direction_command == 'backward':
RL.both_off()
RL.red()
elif direction_command == 'no':
RL.both_off()
elif 'lookleft' == command_input:
P_sc.singleServo(1, 1, 7)
elif 'lookright' == command_input:
P_sc.singleServo(1, -1, 7)
elif 'LRstop' in command_input:
P_sc.stopWiggle()
elif 'up' == command_input:
T_sc.singleServo(0, 1, 7)
elif 'down' == command_input:
T_sc.singleServo(0, -1, 7)
elif 'UDstop' in command_input:
T_sc.stopWiggle()
elif 'home' == command_input:
P_sc.moveServoInit([init_pwm1])
T_sc.moveServoInit([init_pwm0])
G_sc.moveServoInit([init_pwm2])
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 turn_right(degrees):
move.moveTank(-100, -100)
time.sleep(.008888 * degrees)
move.motorStop()
def appCommand(data_input):
global direction_command, turn_command, servo_command
# 0riv3r:
# The following code fixes the un-synced app buttons with their respective functions
# ==================================================================================
#
# The commands that arrive from the phone are not sync with the phone app buttons
# original:
# full-arrows on the right side of the app's pannel
# fixing the arrows by changing the arrived data
# arrow drawing | arrived-text | text-changed-to
# ------------- | ------------ ---------------
# < | downStart, downStop | lookLeftStart, lookLeftStop
# > | upStart, upStop | lookRightStart, lookRightStop
# ^ | lookLeftStart, lookLeftStop | upStart, upStop
# v | lookRightStart, lookRightStop | downStart, downStop
# -----------------------------------------------------------------------------------------
# letters | arrived-text | command-changed-to
# ------- | ------------ ---------------
# A | aStart, aStop - begin police/end police
# B | bStart, bStop - start changing/illuminating back lights | servo.ahead()
# C | cStart, cStop - turn off some lights on the raspberry-pi head
# D | dStart, dStop - turn on some lights on the raspberry-pi head
if data_input == 'lookLeftStart\n':
data_input = 'upStart\n'
elif data_input == 'lookRightStart\n':
data_input = 'downStart\n'
elif data_input == 'upStart\n':
data_input = 'lookRightStart\n'
elif data_input == 'downStart\n':
data_input = 'lookLeftStart\n'
elif 'lookLeftStop' in data_input:
data_input = 'upStop\n'
elif 'lookRightStop' in data_input:
data_input = 'downStop\n'
elif 'downStop' in data_input:
data_input = 'lookLeftStop\n'
elif 'upStop' in data_input:
data_input = 'lookRightStop\n'
# ==================================================================================
if data_input == 'forwardStart\n':
direction_command = 'forward'
move.move(app.speedControl(), direction_command)
elif data_input == 'backwardStart\n':
direction_command = 'backward'
move.move(app.speedControl(), direction_command)
elif data_input == 'leftStart\n':
turn_command = 'left'
servo.turnLeft()
move.move(app.speedControl(), direction_command)
elif data_input == 'rightStart\n':
turn_command = 'right'
servo.turnRight()
move.move(app.speedControl(), direction_command)
elif 'forwardStop' in data_input:
if turn_command == 'no':
move.motorStop()
elif 'backwardStop' in data_input:
if turn_command == 'no':
move.motorStop()
elif 'leftStop' in data_input:
turn_command = 'no'
servo.turnMiddle()
move.motorStop()
elif 'rightStop' in data_input:
turn_command = 'no'
servo.turnMiddle()
move.motorStop()
if data_input == 'lookLeftStart\n':
servo_command = 'lookleft'
servo_move.resume()
elif data_input == 'lookRightStart\n':
servo_command = 'lookright'
servo_move.resume()
elif data_input == 'downStart\n':
servo_command = 'down'
servo_move.resume()
elif data_input == 'upStart\n':
servo_command = 'up'
servo_move.resume()
elif 'lookLeftStop' in data_input:
servo_move.pause()
servo_command = 'no'
elif 'lookRightStop' in data_input:
servo_move.pause()
servo_command = 'no'
elif 'downStop' in data_input:
servo_move.pause()
servo_command = 'no'
elif 'upStop' in data_input:
servo_move.pause()
servo_command = 'no'
if data_input == 'aStart\n':
app.btn_A()
elif data_input == 'bStart\n':
app.btn_B()
elif data_input == 'cStart\n':
switch.switch(1,1)
switch.switch(2,1)
switch.switch(3,1)
elif data_input == 'dStart\n':
switch.switch(1,0)
switch.switch(2,0)
switch.switch(3,0)
elif 'aStop' in data_input:
pass
elif 'bStop' in data_input:
pass
elif 'cStop' in data_input:
pass
elif 'dStop' in data_input:
pass
print(data_input)
def _secureMove(self, sleepDistance):
if(ultra.checkdist() > RANGE_MIN):
# print("ultra.checkdist(): " + str(ultra.checkdist()))
move.move(MoveBody.speed, 'forward')
time.sleep(sleepDistance)
move.motorStop()
def pause(self):
self.functionMode = 'none'
move.motorStop()
self.__flag.clear()
def run(self):
global goal_pos, servo_command, init_get, functionMode
while self.__running.isSet():
self.__flag.wait() # 为True时立即返回, 为False时阻塞直到内部的标识位为True后返回
if functionMode != 6:
if servo_command == 'lookleft':
servo.lookleft(servo_speed)
elif servo_command == 'lookright':
servo.lookright(servo_speed)
elif servo_command == 'up':
servo.up(servo_speed)
elif servo_command == 'down':
servo.down(servo_speed)
elif servo_command == 'lookup':
servo.lookup(servo_speed)
elif servo_command == 'lookdown':
servo.lookdown(servo_speed)
elif servo_command == 'grab':
servo.grab(servo_speed)
elif servo_command == 'loose':
servo.loose(servo_speed)
elif servo_command == 'handup':
servo.handUp(servo_speed)
elif servo_command == 'handdown':
servo.handDown(servo_speed)
else:
pass
if functionMode == 4:
if MPU_connection:
try:
accelerometer_data = sensor.get_accel_data()
Y_get = accelerometer_data['y']
X_get = accelerometer_data['x']
tcpCliSock.send(
('OSD %f %f' %
(round(X_get, 2), round(Y_get, 2))).encode())
#print('OSD %f %f'%(round(Y_get,2),round(X_get,2)))
time.sleep(0.1)
except:
print('MPU_6050 I/O ERROR')
pass
elif functionMode == 5:
servo.ahead()
dis_get = ultra.checkdist()
if dis_get < 0.15:
move.motorStop()
move.move(100, 'backward', 'no', 1)
move.motorStop()
move.move(100, 'no', 'left', 1)
time.sleep(1)
move.motorStop()
else:
move.move(100, 'forward', 'no', 1)
if not functionMode:
move.motorStop()
elif functionMode == 6:
if MPU_connection:
xGet = sensor.get_accel_data()
print(xGet)
xGet = xGet['x']
xOut = kalman_filter_X.kalman(xGet)
pwm.set_pwm(4, 0, servo.pwm3_pos + pwmGenOut(xOut * 9))
# pwm.set_pwm(2, 0, self.steadyGoal+pwmGenOut(xGet*10))
time.sleep(0.05)
continue
else:
functionMode = 0
servo_move.pause()
time.sleep(0.07)
elif self.functionMode == 'Steady':
self.steadyProcessing()
elif self.functionMode == 'trackLine':
self.trackLineProcessing()
elif self.functionMode == 'speechRecProcessing':
self.speechRecProcessing()
elif self.functionMode == 'keepDistance':
self.keepDisProcessing()
def run(self):
while 1:
self.__flag.wait()
self.functionGoing()
pass
if __name__ == '__main__':
pass
try:
fuc = Functions()
# fuc.radarScan()
# fuc.start()
# fuc.automatic()
fuc.trackLine()
# # fuc.steady(300)
# time.sleep(30)
# fuc.pause()
# time.sleep(1)
# move.move(80, 'no', 'no', 0.5)
except KeyboardInterrupt:
move.motorStop()
def appCommand(data_input):
global direction_command, turn_command, servo_command
if data_input == 'forwardStart\n':
direction_command = 'forward'
move.move(speed_set, direction_command)
elif data_input == 'backwardStart\n':
direction_command = 'backward'
move.move(speed_set, direction_command)
elif data_input == 'leftStart\n':
turn_command = 'left'
servo.turnLeft()
move.move(speed_set, direction_command)
elif data_input == 'rightStart\n':
turn_command = 'right'
servo.turnRight()
move.move(speed_set, direction_command)
elif 'forwardStop' in data_input:
if turn_command == 'no':
move.motorStop()
elif 'backwardStop' in data_input:
if turn_command == 'no':
move.motorStop()
elif 'leftStop' in data_input:
turn_command = 'no'
servo.turnMiddle()
move.motorStop()
elif 'rightStop' in data_input:
turn_command = 'no'
servo.turnMiddle()
move.motorStop()
if data_input == 'lookLeftStart\n':
servo_command = 'lookleft'
servo_move.resume()
elif data_input == 'lookRightStart\n':
servo_command = 'lookright'
servo_move.resume()
elif data_input == 'downStart\n':
servo_command = 'down'
servo_move.resume()
elif data_input == 'upStart\n':
servo_command = 'up'
servo_move.resume()
elif 'lookLeftStop' in data_input:
servo_move.pause()
servo_command = 'no'
elif 'lookRightStop' in data_input:
servo_move.pause()
servo_command = 'no'
elif 'downStop' in data_input:
servo_move.pause()
servo_command = 'no'
elif 'upStop' in data_input:
servo_move.pause()
servo_command = 'no'
if data_input == 'aStart\n':
if LED.ledfunc != 'police':
LED.ledfunc = 'police'
ledthread.resume()
elif LED.ledfunc == 'police':
LED.ledfunc = ''
ledthread.pause()
elif data_input == 'bStart\n':
if LED.ledfunc != 'rainbow':
LED.ledfunc = 'rainbow'
ledthread.resume()
elif LED.ledfunc == 'rainbow':
LED.ledfunc = ''
ledthread.pause()
elif data_input == 'cStart\n':
switch.switch(1, 1)
switch.switch(2, 1)
switch.switch(3, 1)
elif data_input == 'dStart\n':
switch.switch(1, 0)
switch.switch(2, 0)
switch.switch(3, 0)
elif 'aStop' in data_input:
pass
elif 'bStop' in data_input:
pass
elif 'cStop' in data_input:
pass
elif 'dStop' in data_input:
pass
print(data_input)
def capture_thread(self, IPinver):
global frame_image, camera # Z
font = cv2.FONT_HERSHEY_SIMPLEX
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()
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()
if FindLineMode: # 2
cvFindLine()
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)
# move.move(70, 'no', 'left', 0.6)
X_lock = 0
elif X > (330 + tor * 3):
error = (X - 240) / 5
outv = int(round((pid.GenOut(error)), 0))
servo.lookright(outv)
# move.move(70, 'no', 'right', 0.6)
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)
else:
switch.switch(1, 0)
switch.switch(2, 0)
switch.switch(3, 0)
# 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()
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)
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
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 and not frameRender: # 2
encoded, buffer = cv2.imencode(".jpg", frame_findline)
else:
cv2.putText(
frame_image,
modeText,
(40, 100),
font,
0.5,
(255, 255, 255),
1,
cv2.LINE_AA,
)
encoded, buffer = cv2.imencode(".jpg", frame_image)
jpg_as_text = base64.b64encode(buffer)
footage_socket.send(jpg_as_text)
print(jpg_as_text)
rawCapture.truncate(0)
def functionSelect(command_input, response):
global functionMode
if 'scan' == command_input:
if OLED_connection:
screen.screen_show(5, 'SCANNING')
if modeSelect == 'PT':
radar_send = fuc.radarScan()
print(radar_send)
response['title'] = 'scanResult'
response['data'] = radar_send
time.sleep(0.3)
elif 'findColor' == command_input:
if OLED_connection:
screen.screen_show(5, 'FindColor')
if modeSelect == 'PT':
flask_app.modeselect('findColor')
elif 'motionGet' == command_input:
if OLED_connection:
screen.screen_show(5, 'MotionGet')
flask_app.modeselect('watchDog')
elif 'stopCV' == command_input:
flask_app.modeselect('none')
switch.switch(1, 0)
switch.switch(2, 0)
switch.switch(3, 0)
elif 'police' == command_input:
if OLED_connection:
screen.screen_show(5, 'POLICE')
RL.police()
elif 'policeOff' == command_input:
RL.pause()
move.motorStop()
elif 'automatic' == command_input:
if OLED_connection:
screen.screen_show(5, 'Automatic')
if modeSelect == 'PT':
fuc.automatic()
else:
fuc.pause()
elif 'automaticOff' == command_input:
fuc.pause()
move.motorStop()
elif 'trackLine' == command_input:
fuc.trackLine()
if OLED_connection:
screen.screen_show(5, 'TrackLine')
elif 'trackLineOff' == command_input:
fuc.pause()
elif 'steadyCamera' == command_input:
if OLED_connection:
screen.screen_show(5, 'SteadyCamera')
fuc.steady(C_sc.lastPos[4])
elif 'steadyCameraOff' == command_input:
fuc.pause()
move.motorStop()
def appCommand(data_input):
global direction_command, turn_command, servo_command
if data_input == "forwardStart\n":
direction_command = "forward"
move.move(speed_set, direction_command)
elif data_input == "backwardStart\n":
direction_command = "backward"
move.move(speed_set, direction_command)
elif data_input == "leftStart\n":
turn_command = "left"
servo.turnLeft()
move.move(speed_set, direction_command)
elif data_input == "rightStart\n":
turn_command = "right"
servo.turnRight()
move.move(speed_set, direction_command)
elif "forwardStop" in data_input:
if turn_command == "no":
move.motorStop()
elif "backwardStop" in data_input:
if turn_command == "no":
move.motorStop()
elif "leftStop" in data_input:
turn_command = "no"
servo.turnMiddle()
move.motorStop()
elif "rightStop" in data_input:
turn_command = "no"
servo.turnMiddle()
move.motorStop()
if data_input == "lookLeftStart\n":
servo_command = "lookleft"
servo_move.resume()
elif data_input == "lookRightStart\n":
servo_command = "lookright"
servo_move.resume()
elif data_input == "downStart\n":
servo_command = "down"
servo_move.resume()
elif data_input == "upStart\n":
servo_command = "up"
servo_move.resume()
elif "lookLeftStop" in data_input:
servo_move.pause()
servo_command = "no"
elif "lookRightStop" in data_input:
servo_move.pause()
servo_command = "no"
elif "downStop" in data_input:
servo_move.pause()
servo_command = "no"
elif "upStop" in data_input:
servo_move.pause()
servo_command = "no"
if data_input == "aStart\n":
if LED.ledfunc != "police":
LED.ledfunc = "police"
ledthread.resume()
elif LED.ledfunc == "police":
LED.ledfunc = ""
ledthread.pause()
elif data_input == "bStart\n":
if LED.ledfunc != "rainbow":
LED.ledfunc = "rainbow"
ledthread.resume()
elif LED.ledfunc == "rainbow":
LED.ledfunc = ""
ledthread.pause()
elif data_input == "cStart\n":
switch.switch(1, 1)
switch.switch(2, 1)
switch.switch(3, 1)
elif data_input == "dStart\n":
switch.switch(1, 0)
switch.switch(2, 0)
switch.switch(3, 0)
elif "aStop" in data_input:
pass
elif "bStop" in data_input:
pass
elif "cStop" in data_input:
pass
elif "dStop" in data_input:
pass
print(data_input)
def run():
global v_command
# obtain audio from the microphone
r = sr.Recognizer()
with sr.Microphone(device_index=2, sample_rate=48000) as source:
r.record(source, duration=2)
# r.adjust_for_ambient_noise(source)
RL.both_off()
RL.yellow()
print("Command?")
audio = r.listen(source)
RL.both_off()
RL.blue()
try:
v_command = r.recognize_sphinx(
audio,
keyword_entries=[
("forward", 1.0),
("backward", 1.0),
("left", 1.0),
("right", 1.0),
("stop", 1.0),
],
) # You can add your own command here
print(v_command)
RL.both_off()
RL.cyan()
except sr.UnknownValueError:
print("say again")
RL.both_off()
RL.red()
except sr.RequestError as e:
RL.both_off()
RL.red()
pass
# print('pre')
if "forward" in v_command:
scGear.moveAngle(2, 0)
move.motor_left(1, 0, speed_set)
move.motor_right(1, 0, speed_set)
time.sleep(2)
move.motorStop()
elif "backward" in v_command:
scGear.moveAngle(2, 0)
move.motor_left(1, 1, speed_set)
move.motor_right(1, 1, speed_set)
time.sleep(2)
move.motorStop()
elif "left" in v_command:
scGear.moveAngle(2, 45)
move.motor_left(1, 0, speed_set)
move.motor_right(1, 0, speed_set)
time.sleep(2)
move.motorStop()
scGear.moveAngle(2, 0)
elif "right" in v_command:
scGear.moveAngle(2, -45)
move.motor_left(1, 0, speed_set)
move.motor_right(1, 0, speed_set)
time.sleep(2)
move.motorStop()
scGear.moveAngle(2, 0)
elif "stop" in v_command:
move.motorStop()
else:
pass
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'
move.move(speed_set, 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:
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()
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:
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 '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 'CVFL' in data: #1
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 '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 '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())
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 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 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 turn_left(degrees):
move.moveTank(100, 100)
time.sleep(.008888 * degrees)
move.motorStop()