forked from undera/pylgbst
-
Notifications
You must be signed in to change notification settings - Fork 0
/
android_remote.py
92 lines (71 loc) · 2.32 KB
/
android_remote.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
"""
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_speed(sa, sb)
# time.sleep(0.5)
finally:
robot.motor_AB.stop()
udp_sock.close()