You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
102 lines
3.1 KiB
102 lines
3.1 KiB
import sys
|
|
import serial
|
|
import sys
|
|
import time
|
|
|
|
from twisted.internet import reactor
|
|
from twisted.python import log
|
|
from twisted.web.server import Site
|
|
from twisted.web.static import File
|
|
|
|
from autobahn.twisted.websocket import WebSocketServerFactory, \
|
|
WebSocketServerProtocol
|
|
|
|
from autobahn.twisted.resource import WebSocketResource
|
|
|
|
i = 0
|
|
turn_sleeptime = 0.8
|
|
straight_sleeptime = 0.5
|
|
|
|
def setMotor(command):
|
|
|
|
if command == 'D':
|
|
vacuum_serial.write(str.encode(
|
|
"SetMotor LWheelDist 155 RWheelDist 155 Speed 150 \n"))
|
|
time.sleep(straight_sleeptime)
|
|
if command == 'L':
|
|
vacuum_serial.write(str.encode(
|
|
"SetMotor LWheelDist -190 RWheelDist 190 Speed 200 \n"))
|
|
time.sleep(turn_sleeptime)
|
|
if command == 'R':
|
|
vacuum_serial.write(str.encode(
|
|
"SetMotor LWheelDist 190 RWheelDist -190 Speed 200 \n"))
|
|
time.sleep(turn_sleeptime)
|
|
if command == 'F':
|
|
vacuum_serial.write(str.encode(
|
|
"SetMotor LWheelDist -155 RWheelDist -155 Speed 150 \n"))
|
|
time.sleep(straight_sleeptime)
|
|
|
|
def setServo(servoChannel, position):
|
|
servoStr = str(servoChannel) + "=" + str(position) + "\n"
|
|
with open("/dev/servoblaster", "wb") as f:
|
|
f.write(bytes(servoStr, 'utf-8'))
|
|
|
|
|
|
class EchoServerProtocol(WebSocketServerProtocol):
|
|
|
|
def onConnect(self, request):
|
|
print("WebSocket connection request: {}".format(request))
|
|
|
|
def onMessage(self, payload, isBinary):
|
|
global i
|
|
i = i + 1
|
|
# if isBinary:
|
|
# print("Binary message received: {0} bytes".format(len(payload)))
|
|
# else:
|
|
# print("Text message received: {0}".format(payload.decode('utf8')))
|
|
string = " " + str(i)
|
|
self.sendMessage(payload + bytes(string, 'utf-8'), isBinary)
|
|
string = payload.decode('ASCII')
|
|
print("Str ->{0}".format(string))
|
|
if string[0] != 'R' and string[0] != 'L' and string[0] != 'F' and string[0] != 'D':
|
|
setServo(2, int(string.split("/")[0]))
|
|
setServo(1, int(string.split("/")[1]))
|
|
else:
|
|
setMotor(string[0])
|
|
|
|
|
|
if __name__ == '__main__':
|
|
|
|
try:
|
|
|
|
vacuum_serial = serial.Serial('/dev/ttyACM0', 115200)
|
|
vacuum_serial.write(str.encode("testmode on \n"))
|
|
time.sleep(1)
|
|
|
|
log.startLogging(sys.stdout)
|
|
|
|
factory = WebSocketServerFactory(u"ws://10.253.35.143:45684")
|
|
factory.protocol = EchoServerProtocol
|
|
|
|
resource = WebSocketResource(factory)
|
|
|
|
# we server static files under "/" ..
|
|
root = File(".")
|
|
|
|
# and our WebSocket server under "/ws" (note that Twisted uses
|
|
# bytes for URIs)
|
|
root.putChild(b"ws", resource)
|
|
|
|
# both under one Twisted Web Site
|
|
site = Site(root)
|
|
reactor.listenTCP(45684, site)
|
|
|
|
reactor.run()
|
|
|
|
vacuum_serial.write(str.encode("testmode off \n"))
|
|
vacuum_serial.close()
|
|
|
|
except KeyboardInterrupt:
|
|
print("Fuuuuuuuuuuuu")
|
|
vacuum_serial.write(str.encode("testmode off \n"))
|
|
vacuum_serial.close()
|
|
|