class Server: def __init__(self): self.tcp_flag = False self.led = Led() self.adc = ADS7830() self.servo = Servo() self.buzzer = Buzzer() self.control = Control() self.sonic = Ultrasonic() self.control.Thread_conditiona.start() def get_interface_ip(self): s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) return socket.inet_ntoa( fcntl.ioctl(s.fileno(), 0x8915, struct.pack('256s', b'wlan0'[:15]))[20:24]) def turn_on_server(self): #ip adress HOST = self.get_interface_ip() #Port 8002 for video transmission self.server_socket = socket.socket() self.server_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEPORT, 1) self.server_socket.bind((HOST, 8002)) self.server_socket.listen(1) #Port 5002 is used for instruction sending and receiving self.server_socket1 = socket.socket() self.server_socket1.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEPORT, 1) self.server_socket1.bind((HOST, 5002)) self.server_socket1.listen(1) print('Server address: ' + HOST) def turn_off_server(self): try: self.connection.close() self.connection1.close() except: print('\n' + "No client connection") def reset_server(self): self.turn_off_server() self.turn_on_server() self.video = threading.Thread(target=self.transmission_video) self.instruction = threading.Thread(target=self.receive_instruction) self.video.start() self.instruction.start() def send_data(self, connect, data): try: connect.send(data.encode('utf-8')) #print("send",data) except Exception as e: print(e) def transmission_video(self): try: self.connection, self.client_address = self.server_socket.accept() self.connection = self.connection.makefile('wb') except: pass self.server_socket.close() try: with picamera.PiCamera() as camera: camera.resolution = (400, 300) # pi camera resolution camera.framerate = 15 # 15 frames/sec camera.saturation = 80 # Set image video saturation camera.brightness = 50 # Set the brightness of the image (50 indicates the state of white balance) #camera.iso = 400 time.sleep(2) # give 2 secs for camera to initilize start = time.time() stream = io.BytesIO() # send jpeg format video stream print("Start transmit ... ") for foo in camera.capture_continuous(stream, 'jpeg', use_video_port=True): try: self.connection.flush() stream.seek(0) b = stream.read() lengthBin = struct.pack('L', len(b)) self.connection.write(lengthBin) self.connection.write(b) stream.seek(0) stream.truncate() except BaseException as e: #print (e) print("End transmit ... ") break except BaseException as e: #print(e) print("Camera unintall") def receive_instruction(self): try: self.connection1, self.client_address1 = self.server_socket1.accept( ) print("Client connection successful !") except: print("Client connect failed") self.server_socket1.close() while True: try: allData = self.connection1.recv(1024).decode('utf-8') except: if self.tcp_flag: self.reset_server() break else: break if allData == "" and self.tcp_flag: self.reset_server() break else: cmdArray = allData.split('\n') print(cmdArray) if cmdArray[-1] != "": cmdArray == cmdArray[:-1] for oneCmd in cmdArray: data = oneCmd.split("#") if data == None or data[0] == '': continue elif cmd.CMD_BUZZER in data: self.buzzer.run(data[1]) elif cmd.CMD_POWER in data: batteryVoltage = self.adc.batteryPower() command = cmd.CMD_POWER + "#" + str( batteryVoltage[0]) + "#" + str( batteryVoltage[1]) + "\n" #print(command) self.send_data(self.connection1, command) if batteryVoltage[0] < 5.5 or batteryVoltage[1] < 6: for i in range(3): self.buzzer.run("1") time.sleep(0.15) self.buzzer.run("0") time.sleep(0.1) elif cmd.CMD_LED in data: try: stop_thread(thread_led) except: pass thread_led = threading.Thread(target=self.led.light, args=(data, )) thread_led.start() elif cmd.CMD_LED_MOD in data: try: stop_thread(thread_led) #print("stop,yes") except: #print("stop,no") pass thread_led = threading.Thread(target=self.led.light, args=(data, )) thread_led.start() elif cmd.CMD_SONIC in data: command = cmd.CMD_SONIC + "#" + str( self.sonic.getDistance()) + "\n" self.send_data(self.connection1, command) elif cmd.CMD_HEAD in data: if len(data) == 3: self.servo.setServoAngle(int(data[1]), int(data[2])) elif cmd.CMD_CAMERA in data: if len(data) == 3: x = self.control.restriction(int(data[1]), 50, 180) y = self.control.restriction(int(data[2]), 0, 180) self.servo.setServoAngle(0, x) self.servo.setServoAngle(1, y) elif cmd.CMD_RELAX in data: #print(data) if self.control.relax_flag == False: self.control.relax(True) self.control.relax_flag = True else: self.control.relax(False) self.control.relax_flag = False elif cmd.CMD_SERVOPOWER in data: if data[1] == "0": GPIO.output(self.control.GPIO_4, True) else: GPIO.output(self.control.GPIO_4, False) else: self.control.order = data self.control.timeout = time.time() try: stop_thread(thread_led) except: pass try: stop_thread(thread_sonic) except: pass print("close_recv")
from evdev import InputDevice, categorize, ecodes from Motor import * from Buzzer import * dev = InputDevice('/dev/input/event0') print(dev) PWM = Motor() buzzer = Buzzer() for event in dev.read_loop(): if event.type == ecodes.EV_KEY: if event.value == 0: PWM.setMotorModel(0, 0, 0, 0) buzzer.run('0') elif event.value == 1: if event.code == 103: PWM.setMotorModel(1000, 1000, 1000, 1000) elif event.code == 108: PWM.setMotorModel(-1000, -1000, -1000, -1000) elif event.code == 105: PWM.setMotorModel(-1500, -1500, 2000, 2000) elif event.code == 106: PWM.setMotorModel(2000, 2000, -1500, -1500) elif event.code == 28: buzzer.run('1')
class Server: def __init__(self): self.PWM = Motor() self.servo = Servo() self.led = Led() self.ultrasonic = Ultrasonic() self.buzzer = Buzzer() self.adc = Adc() self.light = Light() self.infrared = Line_Tracking() self.tcp_Flag = True self.sonic = False self.Light = False self.Mode = 'one' self.endChar = '\n' self.intervalChar = '#' def get_interface_ip(self): s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) return socket.inet_ntoa( fcntl.ioctl(s.fileno(), 0x8915, struct.pack('256s', b'wlan0'[:15]))[20:24]) def StartTcpServer(self): HOST = str(self.get_interface_ip()) self.server_socket1 = socket.socket() self.server_socket1.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEPORT, 1) self.server_socket1.bind((HOST, 5000)) self.server_socket1.listen(1) self.server_socket = socket.socket() self.server_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEPORT, 1) self.server_socket.bind((HOST, 8000)) self.server_socket.listen(1) print('Server address: ' + HOST) def StopTcpServer(self): try: self.connection.close() self.connection1.close() except Exception as e: print('\n' + "No client connection") def Reset(self): self.StopTcpServer() self.StartTcpServer() self.SendVideo = Thread(target=self.sendvideo) self.ReadData = Thread(target=self.readdata) self.SendVideo.start() self.ReadData.start() def send(self, data): self.connection1.send(data.encode('utf-8')) def sendvideo(self): try: self.connection, self.client_address = self.server_socket.accept() self.connection = self.connection.makefile('wb') except: pass self.server_socket.close() try: with picamera.PiCamera() as camera: camera.resolution = (400, 300) # pi camera resolution camera.framerate = 15 # 15 frames/sec time.sleep(2) # give 2 secs for camera to initilize start = time.time() stream = io.BytesIO() # send jpeg format video stream print("Start transmit ... ") for foo in camera.capture_continuous(stream, 'jpeg', use_video_port=True): try: self.connection.flush() stream.seek(0) b = stream.read() length = len(b) if length > 5120000: continue lengthBin = struct.pack('L', length) self.connection.write(lengthBin) self.connection.write(b) stream.seek(0) stream.truncate() except Exception as e: print(e) print("End transmit ... ") break except: #print "Camera unintall" pass def stopMode(self): try: stop_thread(self.infraredRun) self.PWM.setMotorModel(0, 0, 0, 0) except: pass try: stop_thread(self.lightRun) self.PWM.setMotorModel(0, 0, 0, 0) except: pass try: stop_thread(self.ultrasonicRun) self.PWM.setMotorModel(0, 0, 0, 0) self.servo.setServoPwm('0', 90) self.servo.setServoPwm('1', 90) except: pass def readdata(self): try: try: self.connection1, self.client_address1 = self.server_socket1.accept( ) print("Client connection successful ! ---- SURE?") except: print("Client connect failed") restCmd = "" self.server_socket1.close() while True: try: AllData = restCmd + self.connection1.recv(1024).decode( 'utf-8') except: if self.tcp_Flag: self.Reset() break print('AllData is', AllData) if len(AllData) < 5: restCmd = AllData if restCmd == '' and self.tcp_Flag: self.Reset() break restCmd = "" if AllData == '': break else: cmdArray = AllData.split("\n") if (cmdArray[-1] != ""): restCmd = cmdArray[-1] cmdArray = cmdArray[:-1] for oneCmd in cmdArray: data = oneCmd.split("#") if data == None: continue elif cmd.CMD_MODE in data: if data[1] == 'one': self.stopMode() self.Mode = 'one' elif data[1] == 'two': self.stopMode() self.Mode = 'two' self.lightRun = Thread(target=self.light.run) self.lightRun.start() elif data[1] == 'three': self.stopMode() self.Mode = 'three' self.ultrasonicRun = threading.Thread( target=self.ultrasonic.run) self.ultrasonicRun.start() elif data[1] == 'four': self.stopMode() self.Mode = 'four' self.infraredRun = threading.Thread( target=self.infrared.run) self.infraredRun.start() elif (cmd.CMD_MOTOR in data) and self.Mode == 'one': print('Data is', data) try: data1 = int(data[1]) data2 = int(data[2]) data3 = int(data[3]) data4 = int(data[4]) if data1 == None or data2 == None or data2 == None or data3 == None: continue self.PWM.setMotorModel(data1, data2, data3, data4) except: pass elif cmd.CMD_SERVO in data: try: data1 = data[1] data2 = int(data[2]) if data1 == None or data2 == None: continue self.servo.setServoPwm(data1, data2) except: pass elif cmd.CMD_LED in data: try: data1 = int(data[1]) data2 = int(data[2]) data3 = int(data[3]) data4 = int(data[4]) if data1 == None or data2 == None or data2 == None or data3 == None: continue self.led.ledIndex(data1, data2, data3, data4) except: pass elif cmd.CMD_LED_MOD in data: self.LedMoD = data[1] if self.LedMoD == '0': try: stop_thread(Led_Mode) except: pass self.led.ledMode(self.LedMoD) time.sleep(0.1) self.led.ledMode(self.LedMoD) else: try: stop_thread(Led_Mode) except: pass time.sleep(0.1) Led_Mode = Thread(target=self.led.ledMode, args=(data[1], )) Led_Mode.start() elif cmd.CMD_SONIC in data: if data[1] == '1': self.sonic = True self.ultrasonicTimer = threading.Timer( 0.5, self.sendUltrasonic) self.ultrasonicTimer.start() else: self.sonic = False elif cmd.CMD_BUZZER in data: try: self.buzzer.run(data[1]) except: pass elif cmd.CMD_LIGHT in data: if data[1] == '1': self.Light = True self.lightTimer = threading.Timer( 0.3, self.sendLight) self.lightTimer.start() else: self.Light = False elif cmd.CMD_POWER in data: ADC_Power = self.adc.recvADC(2) * 3 try: self.send(cmd.CMD_POWER + '#' + str(ADC_Power) + '\n') except: pass except Exception as e: print(e) self.StopTcpServer() def sendUltrasonic(self): if self.sonic == True: ADC_Ultrasonic = self.ultrasonic.get_distance() if ADC_Ultrasonic == self.ultrasonic.get_distance(): try: self.send(cmd.CMD_SONIC + "#" + str(ADC_Ultrasonic) + '\n') except: self.sonic = False self.ultrasonicTimer = threading.Timer(0.13, self.sendUltrasonic) self.ultrasonicTimer.start() def sendLight(self): if self.Light == True: ADC_Light1 = self.adc.recvADC(0) ADC_Light2 = self.adc.recvADC(1) try: self.send(cmd.CMD_LIGHT + '#' + str(ADC_Light1) + '#' + str(ADC_Light2) + '\n') except: self.Light = False self.lightTimer = threading.Timer(0.17, self.sendLight) self.lightTimer.start() def Power(self): while True: ADC_Power = self.adc.recvADC(2) * 3 time.sleep(3) if ADC_Power < 6.8: for i in range(4): self.buzzer.run('1') time.sleep(0.1) self.buzzer.run('0') time.sleep(0.1) elif ADC_Power < 7: for i in range(2): self.buzzer.run('1') time.sleep(0.1) self.buzzer.run('0') time.sleep(0.1) else: self.buzzer.run('0')
class localKeyboard: def __init__(self): self.headUpDownAngle = 90 self.headLeftRightAngle = 90 self.PWM = Motor() self.servo = Servo() self.horn = Buzzer() self.speed = 1000 # corrected servo positions # adjust these to suit your car # so head is front and centre at start self.headLRcorrect = -3 self.headUDcorrect = 4 self.reset_head() self.selector = DefaultSelector() # set true for mecanum wheels self.mecanum = False self.useLights = True self.led = Led() self.mouse = evdev.InputDevice('/dev/input/event1') self.keybd = evdev.InputDevice('/dev/input/event0') self.readingKeys = False self.led.colorWipe(self.led.strip, Color(0,0,0),0) self.brake = False self.reverse = False self.indicating = False self.leftTurn = False self.rightTurn = False self.moving = False self.indi_time = datetime.now() self.indi_off = True self.brake_time = datetime.now() self.brake_off = True atexit.register(self.keybd.ungrab) # Don't forget to ungrab the keyboard on exit! atexit.register(self.mouse.ungrab) self.keybd.grab() # Grab, i.e. prevent the keyboard from emitting original events.# self.mouse.grab() # This works because InputDevice has a `fileno()` method. self.selector.register(self.mouse, EVENT_READ) self.selector.register(self.keybd, EVENT_READ) def read_keys_loop(self): self.readingKeys = True while self.readingKeys: self.read_keys() # only manage lights after a key press so brake lights, if on, # will stay on until next key event if self.useLights: self.manage_lights() def manage_lights(self): # indicators if not self.indicating and not self.reverse: self.led.colorWipe(self.led.strip, Color(0,0,0),0) else: if self.indicating: if (datetime.now() - self.indi_time).microseconds > 250000: self.indi_off = not self.indi_off self.indi_time = datetime.now() if self.indi_off: if self.leftTurn: self.led.strip.setPixelColor(2, Color(125,85,0) ) self.led.strip.setPixelColor(5, Color(125,85,0) ) if self.rightTurn: self.led.strip.setPixelColor(1, Color(125,85,0) ) self.led.strip.setPixelColor(6, Color(125,85,0) ) self.led.strip.show() else: self.led.colorWipe(self.led.strip, Color(0,0,0),0) if self.reverse: self.led.strip.setPixelColor(1, Color(255,255,255) ) self.led.strip.setPixelColor(2, Color(255,255,255) ) self.led.strip.show() if self.brake: self.brake = False if self.brake_off: self.brake_off = False self.brake_time = datetime.now() self.led.strip.setPixelColor(1, Color(255,0,0) ) self.led.strip.setPixelColor(2, Color(255,0,0) ) self.led.strip.show() if not self.brake_off: #this is a minimum time on, they stay on until next key press if (datetime.now() - self.brake_time).microseconds > 250000: self.led.colorWipe(self.led.strip, Color(0,0,0),0) self.brake = False self.brake_off = True def read_keys(self): for key, mask in self.selector.select(): device =key.fileobj for event in device.read(): if event.type == evdev.ecodes.EV_KEY: # print("key press") # print(evdev.ecodes.bytype[evdev.ecodes.EV_KEY][event.code]) if event.value == 1 or event.value == 2: self.key_press(event, self.keybd) elif event.value == 0: self.drive_stop() elif event.type == evdev.ecodes.EV_REL: if event.code == evdev.ecodes.REL_X: if event.value < 0: self.head_left() else: self.head_right() if event.code == evdev.ecodes.REL_Y: if event.value < 0: self.head_down() else: self.head_up() else: pass #print(event) def key_press(self, ev, kbd): if (ev.value == 1 or ev.value == 2): # 1 PRESS or 2 HOLD # EVENTS CALLED ON PRESS AND ON HOLD # HEAD POSITION if ev.code == evdev.ecodes.KEY_Z: self.head_down() elif ev.code == evdev.ecodes.KEY_A: self.head_left() elif ev.code == evdev.ecodes.KEY_S: self.head_right() elif ev.code == evdev.ecodes.KEY_W: self.head_up() # HORN elif ev.code == evdev.ecodes.KEY_T: self.toot() # not interested in any other held keys elif ev.value == 2: pass #EVENTS THAT SHOULD ONLY BE CALLED ON PRESS AND NOT HOLD # SPEED SETTING elif ev.code == evdev.ecodes.KEY_1: self.speed = 1000 elif ev.code == evdev.ecodes.KEY_2: self.speed = 1200 elif ev.code == evdev.ecodes.KEY_3: self.speed = 1400 elif ev.code == evdev.ecodes.KEY_4: self.speed = 1700 elif ev.code == evdev.ecodes.KEY_5: self.speed = 2000 elif ev.code == evdev.ecodes.KEY_6: self.speed = 2400 elif ev.code == evdev.ecodes.KEY_7: self.speed = 2800 elif ev.code == evdev.ecodes.KEY_8: self.speed = 3200 elif ev.code == evdev.ecodes.KEY_9: self.speed = 3600 elif ev.code == evdev.ecodes.KEY_0: self.speed = 4000 # DRIVE FUNCTIONS elif ev.code == evdev.ecodes.KEY_UP: self.drive_forward() elif ev.code == evdev.ecodes.KEY_DOWN: self.drive_backward() elif ev.code == evdev.ecodes.KEY_LEFT: self.turn_left() elif ev.code == evdev.ecodes.KEY_RIGHT: self.turn_right() elif ev.code == evdev.ecodes.KEY_COMMA: self.crab_left() elif ev.code == evdev.ecodes.KEY_DOT: self.crab_right() elif ev.code == evdev.ecodes.KEY_SEMICOLON: self.diag_right() elif ev.code == evdev.ecodes.KEY_K: self.diag_left() elif ev.code == evdev.ecodes.KEY_SLASH: self.diag_rev_right() elif ev.code == evdev.ecodes.KEY_M: self.diag_rev_left() elif ev.code == evdev.ecodes.KEY_U: self.curve_right() elif ev.code == evdev.ecodes.KEY_Y: self.curve_left() elif ev.code == evdev.ecodes.KEY_J: self.curve_rev_right() elif ev.code == evdev.ecodes.KEY_H: self.curve_rev_left() # USE OR DONT USE LIGHTS elif ev.code == evdev.ecodes.KEY_L: self.useLights = not self.useLights if not self.useLights: self.led.colorWipe(self.led.strip, Color(0,0,0),0) # RESET TO START STATE elif ev.code == evdev.ecodes.KEY_HOME: #RESET TO START STATE self.drive_stop() self.servo.setServoPwm('0', int(self.headLeftRightAngle)) self.servo.setServoPwm('1', int(self.headUpDownAngle)) self.speed = 1000 self.led.colorWipe(self.led.strip, Color(0,0,0),0) # PROG FUNCTIONS elif ev.code == evdev.ecodes.KEY_LEFTMETA: self.close() elif ev.code == evdev.ecodes.KEY_END: self.shutdown_pi() elif ev.code == evdev.ecodes.KEY_SYSRQ: self.reboot_pi() else: print("UNUSED KEY CODE") print(evdev.ecodes.bytype[evdev.ecodes.EV_KEY][ev.code]) if ev.value == 0: self.drive_stop() # flush backed up key presses while kbd.read_one() is not None: if ev.value == 0 : self.drive_stop() def close(self): self.readingKeys = False self.selector.unregister(self.mouse) self.selector.unregister(self.keybd) self.led.colorWipe(self.led.strip, Color(0,0,0),0) # kbd should be ungrabbed by atexit # but belt and braces try: self.keybd.ungrab self.mouse.ungrab except: pass sys.exit() def shutdown_pi(self): self.readingKeys = False self.toot() time.sleep(0.2) self.toot() call("sudo nohup shutdown -h now", shell=True) def reboot_pi(self): self.readingKeys = False self.toot() call("sudo nohup reboot", shell=True) def toot(self): self.horn.run('1') time.sleep(0.2) self.horn.run('0') def drive_forward(self): self.moving = True PWM.setMotorModel(self.speed, self.speed, self.speed, self.speed) def turn_left(self): self.moving = True self.indicating = True self.leftTurn = True self.rightTurn = False PWM.setMotorModel(-self.speed, -self.speed, self.speed, self.speed) def drive_backward(self): self.moving = True self.reverse = True PWM.setMotorModel(-self.speed, -self.speed, -self.speed, -self.speed) def turn_right(self): self.moving = True self.indicating = True self.leftTurn = False self.rightTurn = True PWM.setMotorModel(self.speed, self.speed, -self.speed, -self.speed) def curve_left(self, biasPcent=20): self.moving = True PWM.setMotorModel(int(self.speed * (100 - biasPcent) / 100), int(self.speed * (100 - biasPcent) / 100), int(self.speed * (100 + biasPcent) / 100), int(self.speed * (100 + biasPcent) / 100)) def curve_right(self, biasPcent=20): self.moving = True PWM.setMotorModel(int(self.speed * (100 + biasPcent) / 100), int(self.speed * (100 + biasPcent) / 100), int(self.speed * (100 - biasPcent) / 100), int(self.speed * (100 - biasPcent) / 100)) def curve_rev_left(self, biasPcent=20): self.moving = True self.reverse = True PWM.setMotorModel(-int(self.speed * (100 - biasPcent) / 100), -int(self.speed * (100 - biasPcent) / 100), -int(self.speed * (100 + biasPcent) / 100), -int(self.speed * (100 + biasPcent) / 100)) def curve_rev_right(self, biasPcent=20): self.moving = True self.reverse = True PWM.setMotorModel(-int(self.speed * (100 + biasPcent) / 100), -int(self.speed * (100 + biasPcent) / 100), -int(self.speed * (100 - biasPcent) / 100), -int(self.speed * (100 - biasPcent) / 100)) def crab_left(self): #REQUIRES MECANUM WHEELS if self.mecanum: self.moving = True self.indicating = True self.leftTurn = True self.rightTurn = False PWM.setMotorModel(-self.speed, self.speed, self.speed, -self.speed) def crab_right(self): #REQUIRES MECANUM WHEELS if self.mecanum: self.moving = True self.indicating = True self.leftTurn = False self.rightTurn = True PWM.setMotorModel(self.speed, -self.speed, -self.speed, self.speed) def diag_right(self): #REQUIRES MECANUM WHEELS if self.mecanum: self.moving = True PWM.setMotorModel(self.speed, 0, 0, self.speed) def diag_left(self): #REQUIRES MECANUM WHEELS if self.mecanum: self.moving = True PWM.setMotorModel(0, self.speed, self.speed, 0) def diag_rev_left(self): #REQUIRES MECANUM WHEELS if self.mecanum: self.moving = True self.reverse = True PWM.setMotorModel(-self.speed, 0, 0, -self.speed) def diag_rev_right(self): #REQUIRES MECANUM WHEELS if self.mecanum: self.moving = True self.reverse = True PWM.setMotorModel(0, -self.speed, -self.speed, 0) def drive_stop(self): if self.moving: self.brake = True self.moving = False PWM.setMotorModel(0, 0, 0, 0) self.reverse = False self.indicating = False self.leftTurn = False self.rightTurn = False def head_up(self): self.headUpDownAngle += 1 if self.headUpDownAngle > 180 + self.headUDcorrect: self.headUpDownAngle = 180 + self.headUDcorrect self.servo.setServoPwm('1', self.headUpDownAngle) # print("Up/down " + str(self.headUpDownAngle)) def head_down(self): self.headUpDownAngle -= 1 if self.headUpDownAngle < 80 + self.headUDcorrect: self.headUpDownAngle = 80 + self.headUDcorrect self.servo.setServoPwm('1', self.headUpDownAngle) # print("Up/down " + str(self.headUpDownAngle)) def head_left(self): self.headLeftRightAngle -= 1 if self.headLeftRightAngle < 10 + self.headLRcorrect: self.headLeftRightAngle = 10 + self.headLRcorrect self.servo.setServoPwm('0', self.headLeftRightAngle) # print("Left/Right " + str(self.headLeftRightAngle)) def head_LRpos(self, angle): # print("Move head to " + str(self.headLeftRightAngle)) self.headLeftRightAngle = angle + self.headLRcorrect self.servo.setServoPwm('0', self.headLeftRightAngle) def head_right(self): self.headLeftRightAngle += 1 if self.headLeftRightAngle > 170 + self.headLRcorrect: self.headLeftRightAngle = 170 + self.headLRcorrect self.servo.setServoPwm('0', self.headLeftRightAngle) # print("Left/Right " + str(self.headLeftRightAngle)) def reset_head(self): self.headLeftRightAngle = 90 + self.headLRcorrect self.headUpDownAngle = 90 + self.headUDcorrect self.servo.setServoPwm('0', int(self.headLeftRightAngle)) self.servo.setServoPwm('1', int(self.headUpDownAngle))
class Server: def __init__(self): self.tcp_flag = False self.led = Led() self.servo = Servo() self.adc = ADS7830() self.buzzer = Buzzer() self.control = Control() self.sonic = Ultrasonic() self.control.Thread_conditiona.start() self.battery_voltage = [8.4, 8.4, 8.4, 8.4, 8.4] def get_interface_ip(self): s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) return socket.inet_ntoa( fcntl.ioctl(s.fileno(), 0x8915, struct.pack('256s', b'wlan0'[:15]))[20:24]) def turn_on_server(self): #ip adress HOST = self.get_interface_ip() #Port 8000 for video transmission self.server_socket = socket.socket() self.server_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEPORT, 1) self.server_socket.bind((HOST, 8001)) self.server_socket.listen(1) #Port 5000 is used for instruction sending and receiving self.server_socket1 = socket.socket() self.server_socket1.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEPORT, 1) self.server_socket1.bind((HOST, 5001)) self.server_socket1.listen(1) print('Server address: ' + HOST) def turn_off_server(self): try: self.connection.close() self.connection1.close() except: print('\n' + "No client connection") def reset_server(self): self.turn_off_server() self.turn_on_server() self.video = threading.Thread(target=self.transmission_video) self.instruction = threading.Thread(target=self.receive_instruction) self.video.start() self.instruction.start() def send_data(self, connect, data): try: connect.send(data.encode('utf-8')) #print("send",data) except Exception as e: print(e) def transmission_video(self): try: self.connection, self.client_address = self.server_socket.accept() self.connection = self.connection.makefile('wb') except: pass self.server_socket.close() try: with picamera.PiCamera() as camera: camera.resolution = (400, 300) # pi camera resolution camera.framerate = 15 # 15 frames/sec camera.saturation = 80 # Set image video saturation camera.brightness = 50 # Set the brightness of the image (50 indicates the state of white balance) start = time.time() stream = io.BytesIO() # send jpeg format video stream print("Start transmit ... ") for foo in camera.capture_continuous(stream, 'jpeg', use_video_port=True): try: self.connection.flush() stream.seek(0) b = stream.read() lengthBin = struct.pack('L', len(b)) self.connection.write(lengthBin) self.connection.write(b) stream.seek(0) stream.truncate() except BaseException as e: #print (e) print("End transmit ... ") break except BaseException as e: #print(e) print("Camera unintall") def measuring_voltage(self, connect): try: for i in range(5): self.battery_voltage[i] = round(self.adc.power(0), 2) command = cmd.CMD_POWER + '#' + str(max( self.battery_voltage)) + "\n" self.send_data(connect, command) self.sednRelaxFlag() self.battery_reminder() except Exception as e: print(e) def battery_reminder(self): if max(self.battery_voltage) < 6.4: self.turn_off_server() self.control.relax(True) print( "The batteries power are too low. Please recharge the batteries or replace batteries." ) print("Close the server") os._exit(0) def sednRelaxFlag(self): if self.control.move_flag != 2: command = cmd.CMD_RELAX + "#" + str(self.control.move_flag) + "\n" self.send_data(self.connection1, command) self.control.move_flag = 2 def receive_instruction(self): try: self.connection1, self.client_address1 = self.server_socket1.accept( ) print("Client connection successful !") except: print("Client connect failed") self.server_socket1.close() while True: try: allData = self.connection1.recv(1024).decode('utf-8') #print(allData) except: if self.tcp_flag: if max(self.battery_voltage) > 6.4: self.reset_server() break else: break if allData == "" and self.tcp_flag: self.reset_server() break else: cmdArray = allData.split('\n') #print(cmdArray) if cmdArray[-1] != "": cmdArray == cmdArray[:-1] for oneCmd in cmdArray: data = oneCmd.split("#") if data == None or data[0] == '': continue elif cmd.CMD_BUZZER in data: self.buzzer.run(data[1]) elif cmd.CMD_LED in data: try: stop_thread(thread_led) except: pass thread_led = threading.Thread(target=self.led.light, args=(data, )) thread_led.start() elif cmd.CMD_LED_MOD in data: try: stop_thread(thread_led) except: pass thread_led = threading.Thread(target=self.led.light, args=(data, )) thread_led.start() elif cmd.CMD_HEAD in data: self.servo.setServoAngle(15, int(data[1])) elif cmd.CMD_SONIC in data: command = cmd.CMD_SONIC + '#' + str( self.sonic.getDistance()) + "\n" self.send_data(self.connection1, command) elif cmd.CMD_POWER in data: self.measuring_voltage(self.connection1) elif cmd.CMD_WORKING_TIME in data: if self.control.move_timeout != 0 and self.control.relax_flag == True: if self.control.move_count > 180: command = cmd.CMD_WORKING_TIME + '#' + str( 180) + '#' + str( round(self.control.move_count - 180)) + "\n" else: if self.control.move_count == 0: command = cmd.CMD_WORKING_TIME + '#' + str( round( self.control.move_count)) + '#' + str( round((time.time() - self.control.move_timeout) + 60)) + "\n" else: command = cmd.CMD_WORKING_TIME + '#' + str( round(self.control.move_count) ) + '#' + str( round(time.time() - self.control.move_timeout)) + "\n" else: command = cmd.CMD_WORKING_TIME + '#' + str( round( self.control.move_count)) + '#' + str(0) + "\n" self.send_data(self.connection1, command) else: self.control.order = data self.control.timeout = time.time() try: stop_thread(thread_power) except: pass try: stop_thread(thread_led) except: pass print("close_recv") self.control.relax_flag = False self.control.order[0] = cmd.CMD_RELAX