def right(self, sleepBeforeDrive, headAngle,
wheelsTurnAngle, sleepDistance):
servo.ahead()
servo.lookright(headAngle)
time.sleep(sleepBeforeDrive)
servo.turnRight(wheelsTurnAngle)
self._secureMove(sleepDistance)
def run(self):
global goal_pos, servo_command, init_get, functionMode
while self.__running.isSet():
self.__flag.wait()
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)
else:
pass
if functionMode == 4:
servo.ahead()
findline.run()
if not functionMode:
move.motorStop()
elif functionMode == 5:
autoDect(50)
if not functionMode:
move.motorStop()
elif functionMode == 6:
if MPU_connection:
accelerometer_data = sensor.get_accel_data()
X_get = accelerometer_data['x']
if not init_get:
goal_pos = X_get
init_get = 1
if servo_command == 'up':
servo.up(servo_speed)
time.sleep(0.2)
accelerometer_data = sensor.get_accel_data()
X_get = accelerometer_data['x']
goal_pos = X_get
elif servo_command == 'down':
servo.down(servo_speed)
time.sleep(0.2)
accelerometer_data = sensor.get_accel_data()
X_get = accelerometer_data['x']
goal_pos = X_get
if abs(X_get - goal_pos) > tor_pos:
if X_get > goal_pos:
servo.down(int(mpu_speed * abs(X_get - goal_pos)))
elif X_get < goal_pos:
servo.up(int(mpu_speed * abs(X_get - goal_pos)))
time.sleep(0.03)
continue
else:
functionMode = 0
try:
self.pause()
except:
pass
time.sleep(0.03)
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 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 FindColor(self, invar):
global FindColorMode
FindColorMode = invar
if not FindColorMode:
servo.ahead()
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)
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 "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)
def FindItem(self, invar): global FindItemMode FindItemMode = invar if not FindItemMode: servo.ahead()
def right(self, sleepTime, headAngle):
servo.ahead()
servo.lookright(headAngle)
time.sleep(sleepTime)
def forward(self, sleepBeforeDrive, sleepDistance):
servo.ahead()
time.sleep(sleepBeforeDrive)
self._secureMove(sleepDistance)
def left(self, sleepTime, headAngle):
servo.ahead()
servo.lookleft(headAngle)
time.sleep(sleepTime)
def ahead(self, sleepTime):
servo.ahead()
time.sleep(sleepTime)
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 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(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)
