1010parameters and upload this sketch. Watch the coordinates change as you move your device around!
1111
1212"""
13- from time import sleep
13+ from time import sleep , time
1414
1515from pypozyx import *
16+ from pypozyx .definitions .registers import POZYX_EUL_HEADING
1617from pythonosc .osc_message_builder import OscMessageBuilder
1718from pythonosc .udp_client import SimpleUDPClient
1819
@@ -33,7 +34,7 @@ def __init__(self, pozyx, osc_udp_client, tags, anchors, algorithm=POZYX_POS_ALG
3334
3435 def setup (self ):
3536 """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: " )
3738 self .setAnchorsManual ()
3839 self .printPublishAnchorConfiguration ()
3940
@@ -48,33 +49,38 @@ def loop(self):
4849 else :
4950 self .printPublishErrorCode ("positioning" , tag )
5051
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+
5162 def setAnchorsManual (self ):
5263 """Adds the manually measured anchors to the Pozyx's device list one for one."""
5364 for tag in self .tags :
5465 status = self .pozyx .clearDevices (tag )
5566 for anchor in self .anchors :
5667 status &= self .pozyx .addDevice (anchor , tag )
5768 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 )
6074
6175 def printPublishConfigurationResult (self , status , tag_id ):
6276 """Prints the configuration explicit result, prints and publishes error if one occurs"""
77+ if tag_id is None :
78+ tag_id = 0
6379 if status == POZYX_SUCCESS :
6480 print ("Configuration of tag %s: success" % tag_id )
6581 else :
6682 self .printPublishErrorCode ("configuration" , tag_id )
6783
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-
7884 def printPublishErrorCode (self , operation , network_id ):
7985 """Prints the Pozyx's error and possibly sends it as a OSC packet"""
8086 error_code = SingleRegister ()
@@ -83,10 +89,10 @@ def printPublishErrorCode(self, operation, network_id):
8389 network_id = 0
8490 if status == POZYX_SUCCESS :
8591 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 )))
8793 if self .osc_udp_client is not None :
8894 self .osc_udp_client .send_message (
89- "/error_%s" % [ operation , network_id , error_code [0 ]])
95+ "/error_%s" % operation , [ network_id , error_code [0 ]])
9096 else :
9197 # should only happen when not being able to communicate with a remote Pozyx.
9298 self .pozyx .getErrorCode (error_code )
@@ -96,10 +102,10 @@ def printPublishErrorCode(self, operation, network_id):
96102
97103 def printPublishAnchorConfiguration (self ):
98104 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 )))
100106 if self .osc_udp_client is not None :
101107 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 ])
103109 sleep (0.025 )
104110
105111
@@ -112,19 +118,23 @@ def printPublishAnchorConfiguration(self):
112118 if not remote :
113119 remote_id = None
114120
115- use_processing = False # enable to send position data through OSC
121+ use_processing = True # enable to send position data through OSC
116122 ip = "127.0.0.1" # IP for the OSC UDP
117123 network_port = 8888 # network port for the OSC UDP
118124 osc_udp_client = None
119125 if use_processing :
120126 osc_udp_client = SimpleUDPClient (ip , network_port )
121127
122- tags = [0x0001 , 0x0002 , 0x0003 ] # remote tags
128+ #tags = [0x6055]
129+ tags = [0x607a ]
130+ # tags = [0x6055, 0x607a] # remote tags
123131 # 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 ))]
128138
129139 algorithm = POZYX_POS_ALG_UWB_ONLY # positioning algorithm to use
130140 dimension = POZYX_3D # positioning dimension
0 commit comments