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:
led.setColor(255,78,0)
led.both_off()
led.red()
print('locked')
else:
led.setColor(0,78,255)
led.both_off()
led.blue()
print('unlocked')
else:
self.findColorDetection = 0
move.motorStop()
self.pause()
def keepDisProcessing(self):
print('keepDistanceProcessing')
distanceGet = ultra.checkdist()
if distanceGet > (self.rangeKeep/2+0.1):
move.motor_left(1, 0, 80)
move.motor_right(1, 0, 80)
elif distanceGet < (self.rangeKeep/2-0.1):
move.motor_left(1, 1, 80)
move.motor_right(1, 1, 80)
else:
move.motorStop()
def findLineCtrl(self, posInput, setCenter):#2
if posInput:
if posInput > (setCenter + findLineError):
# move.motorStop()
#turnRight
error = (posInput-320)/3
outv = int(round((pid.GenOut(error)),0))
CVThread.scGear.moveAngle(2,-outv)
if CVRun:
move.motor_left(1, 0, 80)
move.motor_right(1, 0, 80)
else:
move.motorStop()
pass
elif posInput < (setCenter - findLineError):
# move.motorStop()
#turnLeft
error = (320-posInput)/3
outv = int(round((pid.GenOut(error)),0))
CVThread.scGear.moveAngle(2,outv)
if CVRun:
move.motor_left(1, 0, 80)
move.motor_right(1, 0, 80)
else:
move.motorStop()
pass
else:
if CVRun:
move.motor_left(1, 0, 80)
move.motor_right(1, 0, 80)
else:
move.motorStop()
#forward
pass
else:
pass
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 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)
move.motorStop()
elif 'KD' == command_input:
if OLED_connection:
screen.screen_show(5,'POLICE')
servoPosInit()
fuc.keepDistance()
RL.police()
elif 'automaticOff' == command_input:
RL.pause()
fuc.pause()
move.motorStop()
time.sleep(0.3)
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()
time.sleep(0.2)
move.motorStop()
elif 'trackLine' == command_input:
servoPosInit()
fuc.trackLine()
if OLED_connection:
screen.screen_show(5,'TrackLine')
elif 'trackLineOff' == command_input:
fuc.pause()
move.motorStop()
elif 'steadyCamera' == command_input:
if OLED_connection:
screen.screen_show(5,'SteadyCamera')
fuc.steady(T_sc.lastPos[2])
elif 'steadyCameraOff' == command_input:
fuc.pause()
move.motorStop()
elif 'speech' == command_input:
RL.both_off()
fuc.speech()
elif 'speechOff' == command_input:
RL.both_off()
fuc.pause()
move.motorStop()
time.sleep(0.3)
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 pause(self): self.functionMode = 'none' move.motorStop() self.__flag.clear()