mirror of
https://github.com/undera/pylgbst.git
synced 2020-11-18 19:37:26 -08:00
32eecac1a6
* It's HUB ID * Rename file * Working with official doc * Some progress * AttachedIO msg * device action impl * some better device alert impl * restructuring * Some port commands handled * Some command feedback waiting * Some more request-reply things * Some more request-reply things * Reworked msg classes * Getting back to UTs * Getting back to UTs * Facing sync lock problems * Facing sync lock problems * Testing it * Covering more with tests * handle actions * Hub class is almost covered * done coverage for Hub * done coverage for MoveHub * Button is tested * remove debug server from examples * Current and voltage tested * color sensor basic test * cover tilt sensor * motor sensor tested * constant motor * motor is tested * hold_speed impl for motor * motor commands recorded * some cleanup * some cleanup * some cleanup * debug * debug * FIX a bug * acc/dec profiles figured out * UT motor ops * UT motor ops * Get rid of weird piggyback * fix UT * Fix encoding? * fix test mb * More robust * Checked demo works * cosmetics * cosmetics * Maybe better test * fetching and decoding some device caps * describing devs * describing devs works * Applying modes we've learned * Simple and extensible dev attach * Reworking peripherals based on modes * Applying modes we've learned * implemented getting sensor data * fixed port subscribe * Added led out cmds on vision sensor * Worked on color-distance sensor * Introduce some locking for consistency * Improved it all * Travis flags * improve * improve * improve docs
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 field.
|
|
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 VisionSensor
|
|
|
|
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.vision_sensor.subscribe(on_distance, VisionSensor.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.start_power(sa, sb)
|
|
# time.sleep(0.5)
|
|
finally:
|
|
robot.motor_AB.stop()
|
|
udp_sock.close()
|