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)