mirror of
https://github.com/undera/pylgbst.git
synced 2020-11-18 19:37:26 -08:00
93 lines
2.3 KiB
Python
93 lines
2.3 KiB
Python
"""
|
|
To use it, install this android app:
|
|
https://play.google.com/store/apps/details?id=com.mscino.sensornode
|
|
Then open app on phone and choose "Stream" => "Stream live data (XML)".
|
|
Check the "Accelerometer" option and put your IP address into corresponding filed.
|
|
Specify port there as 8999, and enable streaming. Then run this script on computer.
|
|
"""
|
|
import logging
|
|
import socket
|
|
import time
|
|
|
|
from examples.vernie import Vernie
|
|
from pylgbst.peripherals import ColorDistanceSensor
|
|
|
|
host = ''
|
|
port = 8999
|
|
running = True
|
|
|
|
|
|
def on_btn(pressed):
|
|
global running
|
|
if pressed:
|
|
running = False
|
|
|
|
|
|
def decode_xml(msg):
|
|
parts = msg.split("</")
|
|
xxx = float(parts[1][parts[1].rfind('>') + 1:])
|
|
yyy = float(parts[2][parts[2].rfind('>') + 1:])
|
|
zzz = float(parts[3][parts[3].rfind('>') + 1:])
|
|
return ranged(xxx), ranged(yyy), ranged(zzz)
|
|
|
|
|
|
def ranged(param):
|
|
return float(param / 10)
|
|
|
|
|
|
front_distance = 0
|
|
|
|
|
|
def on_distance(distance):
|
|
logging.info("Distance %s", distance)
|
|
global front_distance
|
|
front_distance = distance
|
|
|
|
|
|
udp_sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
|
|
udp_sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
|
|
udp_sock.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1)
|
|
udp_sock.settimeout(0)
|
|
|
|
logging.basicConfig(level=logging.INFO)
|
|
robot = Vernie()
|
|
robot.button.subscribe(on_btn)
|
|
robot.motor_AB.stop()
|
|
|
|
robot.color_distance_sensor.subscribe(on_distance, ColorDistanceSensor.DISTANCE_INCHES)
|
|
try:
|
|
udp_sock.bind((host, port))
|
|
time.sleep(1)
|
|
|
|
while running:
|
|
message = ""
|
|
while True:
|
|
try:
|
|
data = udp_sock.recv(8192)
|
|
message = data
|
|
except KeyboardInterrupt:
|
|
raise
|
|
except BaseException:
|
|
break
|
|
|
|
if not message:
|
|
time.sleep(0.1)
|
|
continue
|
|
|
|
messageString = message.decode("utf-8")
|
|
a, b, c = decode_xml(messageString)
|
|
divider = 2.0 if c > 0 else -2.0
|
|
|
|
if 0 < front_distance < 9 and c > 0:
|
|
logging.info("Something in front of Vernie [%s]!", front_distance)
|
|
c = 0
|
|
|
|
sa = round(c + b / divider, 1)
|
|
sb = round(c - b / divider, 1)
|
|
logging.info("SpeedA=%s, SpeedB=%s", sa, sb)
|
|
robot.motor_AB.constant(sa, sb)
|
|
# time.sleep(0.5)
|
|
finally:
|
|
robot.motor_AB.stop()
|
|
udp_sock.close()
|