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