10
10
parameters and upload this sketch. Watch the coordinates change as you move your device around!
11
11
12
12
"""
13
- from time import sleep
13
+ from time import sleep , time
14
14
15
15
from pypozyx import *
16
+ from pypozyx .definitions .registers import POZYX_EUL_HEADING
16
17
from pythonosc .osc_message_builder import OscMessageBuilder
17
18
from pythonosc .udp_client import SimpleUDPClient
18
19
@@ -33,7 +34,7 @@ def __init__(self, pozyx, osc_udp_client, tags, anchors, algorithm=POZYX_POS_ALG
33
34
34
35
def setup (self ):
35
36
"""Sets up the Pozyx for positioning by calibrating its anchor list."""
36
- print ("------------POZYX MULTITAG POSITIONING V1.0 - -----------\n NOTES: \n - Parameters required:\n \t - Anchors for calibration\n \t - Tags to work with\n \n - System will manually calibration\n \n - System will auto start positioning\n - -----------POZYX MULTITAG POSITIONING V1.0 ------------\n START Positioning: " )
37
+ print ("------------POZYX MULTITAG POSITIONING V1.1 - -----------\n NOTES: \n - Parameters required:\n \t - Anchors for calibration\n \t - Tags to work with\n \n - System will manually calibration\n \n - System will auto start positioning\n - -----------POZYX MULTITAG POSITIONING V1.1 ------------\n START Positioning: " )
37
38
self .setAnchorsManual ()
38
39
self .printPublishAnchorConfiguration ()
39
40
@@ -48,33 +49,38 @@ def loop(self):
48
49
else :
49
50
self .printPublishErrorCode ("positioning" , tag )
50
51
52
+ def printPublishPosition (self , position , network_id ):
53
+ """Prints the Pozyx's position and possibly sends it as a OSC packet"""
54
+ if network_id is None :
55
+ network_id = 0
56
+ s = "POS ID: {}, x(mm): {}, y(mm): {}, z(mm): {}" .format ("0x%0.4x" % network_id , position .x , position .y , position .z )
57
+ print (s )
58
+ if self .osc_udp_client is not None :
59
+ self .osc_udp_client .send_message (
60
+ "/position" , [network_id , position .x , position .y , position .z ])
61
+
51
62
def setAnchorsManual (self ):
52
63
"""Adds the manually measured anchors to the Pozyx's device list one for one."""
53
64
for tag in self .tags :
54
65
status = self .pozyx .clearDevices (tag )
55
66
for anchor in self .anchors :
56
67
status &= self .pozyx .addDevice (anchor , tag )
57
68
if len (anchors ) > 4 :
58
- status &= self .pozyx .setSelectionOfAnchors (POZYX_ANCHOR_SEL_AUTO , len (anchors ))
59
- self .printConfigurationResult (status , tag )
69
+ status &= self .pozyx .setSelectionOfAnchors (POZYX_ANCHOR_SEL_AUTO , len (anchors ), remote_id = tag )
70
+ # enable these if you want to save the configuration to the devices.
71
+ # self.pozyx.saveAnchorIds(tag)
72
+ # self.pozyx.saveRegisters([POZYX_ANCHOR_SEL_AUTO], tag)
73
+ self .printPublishConfigurationResult (status , tag )
60
74
61
75
def printPublishConfigurationResult (self , status , tag_id ):
62
76
"""Prints the configuration explicit result, prints and publishes error if one occurs"""
77
+ if tag_id is None :
78
+ tag_id = 0
63
79
if status == POZYX_SUCCESS :
64
80
print ("Configuration of tag %s: success" % tag_id )
65
81
else :
66
82
self .printPublishErrorCode ("configuration" , tag_id )
67
83
68
- def printPublishPosition (self , position , network_id ):
69
- """Prints the Pozyx's position and possibly sends it as a OSC packet"""
70
- if network_id is None :
71
- network_id = 0
72
- print ("POS ID {}, x(mm): {pos.x}, y(mm): {pos.y}, z(mm): {pos.z}" .format (
73
- "0x%0.4x" % network_id , pos = pos ))
74
- if self .osc_udp_client is not None :
75
- self .osc_udp_client .send_message (
76
- "/position" , [network_id , anchor_coordinates .x , anchor_coordinates .y , anchor_coordinates .z ])
77
-
78
84
def printPublishErrorCode (self , operation , network_id ):
79
85
"""Prints the Pozyx's error and possibly sends it as a OSC packet"""
80
86
error_code = SingleRegister ()
@@ -83,10 +89,10 @@ def printPublishErrorCode(self, operation, network_id):
83
89
network_id = 0
84
90
if status == POZYX_SUCCESS :
85
91
print ("Error %s on ID %s, error code %s" %
86
- (operation , "0x%0.4x" % ( network_id , str (error_code ) )))
92
+ (operation , "0x%0.4x" % network_id , str (error_code )))
87
93
if self .osc_udp_client is not None :
88
94
self .osc_udp_client .send_message (
89
- "/error_%s" % [ operation , network_id , error_code [0 ]])
95
+ "/error_%s" % operation , [ network_id , error_code [0 ]])
90
96
else :
91
97
# should only happen when not being able to communicate with a remote Pozyx.
92
98
self .pozyx .getErrorCode (error_code )
@@ -96,10 +102,10 @@ def printPublishErrorCode(self, operation, network_id):
96
102
97
103
def printPublishAnchorConfiguration (self ):
98
104
for anchor in self .anchors :
99
- print ("ANCHOR,0x%0.4x,%s" % (anchor .network_id , str (anchor .coordinates )))
105
+ print ("ANCHOR,0x%0.4x,%s" % (anchor .network_id , str (anchor .pos )))
100
106
if self .osc_udp_client is not None :
101
107
self .osc_udp_client .send_message (
102
- "/anchor" , [anchor .network_id , anchor .coordinates .x , anchor .coordinates .y , anchor .coordinates .z ])
108
+ "/anchor" , [anchor .network_id , anchor .pos .x , anchor .pos .y , anchor .pos .z ])
103
109
sleep (0.025 )
104
110
105
111
@@ -112,19 +118,23 @@ def printPublishAnchorConfiguration(self):
112
118
if not remote :
113
119
remote_id = None
114
120
115
- use_processing = False # enable to send position data through OSC
121
+ use_processing = True # enable to send position data through OSC
116
122
ip = "127.0.0.1" # IP for the OSC UDP
117
123
network_port = 8888 # network port for the OSC UDP
118
124
osc_udp_client = None
119
125
if use_processing :
120
126
osc_udp_client = SimpleUDPClient (ip , network_port )
121
127
122
- tags = [0x0001 , 0x0002 , 0x0003 ] # remote tags
128
+ #tags = [0x6055]
129
+ tags = [0x607a ]
130
+ # tags = [0x6055, 0x607a] # remote tags
123
131
# necessary data for calibration
124
- anchors = [DeviceCoordinates (0x0001 , 1 , Coordinates (0 , 0 , 2000 )),
125
- DeviceCoordinates (0x0002 , 1 , Coordinates (3000 , 0 , 2000 )),
126
- DeviceCoordinates (0x0003 , 1 , Coordinates (0 , 3000 , 2000 )),
127
- DeviceCoordinates (0x0004 , 1 , Coordinates (3000 , 3000 , 2000 ))]
132
+ anchors = [DeviceCoordinates (0x6058 , 1 , Coordinates (0 , 0 , 2210 )),
133
+ DeviceCoordinates (0x6005 , 1 , Coordinates (9200 , - 700 , 2400 )),
134
+ DeviceCoordinates (0x605C , 1 , Coordinates (0 , 17000 , 2470 )),
135
+ DeviceCoordinates (0x6027 , 1 , Coordinates (2700 , 17000 , 2470 )),
136
+ DeviceCoordinates (0x682e , 1 , Coordinates (2700 , 2500 , 2350 )),
137
+ DeviceCoordinates (0x6044 , 1 , Coordinates (11230 , 2750 , 2300 ))]
128
138
129
139
algorithm = POZYX_POS_ALG_UWB_ONLY # positioning algorithm to use
130
140
dimension = POZYX_3D # positioning dimension
0 commit comments