status = POZYX_SUCCESS
ids = [self.remote_id, self.destination_id]
# set the leds of both local/remote and destination pozyx device
for id in ids:
status &= self.pozyx.setLed(4, (distance < range_step_mm), id)
status &= self.pozyx.setLed(3, (distance < 2 * range_step_mm), id)
status &= self.pozyx.setLed(2, (distance < 3 * range_step_mm), id)
status &= self.pozyx.setLed(1, (distance < 4 * range_step_mm), id)
return status
if __name__ == "__main__":
# Check for the latest PyPozyx version. Skip if this takes too long or is not needed by setting to False.
check_pypozyx_version = True
if check_pypozyx_version:
perform_latest_version_check()
# hardcoded way to assign a serial port of the Pozyx
serial_port = 'COM12'
# the easier way
serial_port = get_first_pozyx_serial_port()
if serial_port is None:
print("No Pozyx connected. Check your USB cable or your driver!")
quit()
remote_id = 0x605D # the network ID of the remote device
remote = False # whether to use the given remote device for ranging
if not remote:
remote_id = None
class MultitagPositioning(object):
"""Continuously performs multitag positioning"""
def nicksThing():
def __init__(self,
pozyx,
osc_udp_client,
tag_ids,
anchors,
algorithm=PozyxConstants.POSITIONING_ALGORITHM_UWB_ONLY,
dimension=PozyxConstants.DIMENSION_3D,
height=1000):
self.pozyx = pozyx
self.osc_udp_client = osc_udp_client
self.tag_ids = tag_ids
self.anchors = anchors
self.algorithm = algorithm
self.dimension = dimension
self.height = height
def setup(self):
"""Sets up the Pozyx for positioning by calibrating its anchor list."""
print("------------POZYX MULTITAG POSITIONING V{} -------------".
format(version))
print("")
print(" - System will manually calibrate the tags")
print("")
print(" - System will then auto start positioning")
print("")
if None in self.tag_ids:
for device_id in self.tag_ids:
self.pozyx.printDeviceInfo(device_id)
else:
for device_id in [None] + self.tag_ids:
self.pozyx.printDeviceInfo(device_id)
print("")
print("------------POZYX MULTITAG POSITIONING V{} -------------".
format(version))
print("")
self.setAnchorsManual()
self.printPublishAnchorConfiguration()
def loop(self):
"""Performs positioning and prints the results."""
for tag_id in self.tag_ids:
position = Coordinates()
status = self.pozyx.doPositioning(position,
self.dimension,
self.height,
self.algorithm,
remote_id=tag_id)
if status == POZYX_SUCCESS:
self.printPublishPosition(position, tag_id)
else:
self.printPublishErrorCode("positioning", tag_id)
# WORK IN PROGRESS: First attempt at making a function to collect and store the x, y, and z coordinates globally
"""def giveUsDemCoordyBois(self, position, network_id):
#hopefully gives us the coordinates to be able to access outside of the class
x_position = position.x;
y_position = position.y;
z_position = position.z;
print(x_position);
print(y_position);
print(z_position);"""
def printPublishPosition(self, position, network_id):
"""Prints the Pozyx's position and possibly sends it as a OSC packet"""
if network_id is None:
network_id = 0
s = "POS ID: {}, x(mm): {}, y(mm): {}, z(mm): {}".format(
"0x%0.4x" % network_id, position.x, position.y, position.z)
print(s)
if self.osc_udp_client is not None:
self.osc_udp_client.send_message(
"/position",
[network_id, position.x, position.y, position.z])
#NOTE: front tag is the tag connected to the Pi(0x673c/0x0000) and back tag is connected to external power source
if network_id == 0x0000: #if the tag that is having its coordinates measured is the one connected to the Pi
global x_position_front #indicate to program that the global variable for the front tag's x-position is to be used
global y_position_front #indicate to program that the global variable for the front tag's y-position is to be used
global z_position_front #indicate to program that the global variable for the front tag's z-position is to be used
x_position_front = position.x
#set the x-position of the front tag to the x-value output by the Pozyx
y_position_front = position.y
#set the y-position of the front tag to the y-value output by the Pozyx
z_position_front = position.z
#set the z-position of the front tag to the z-value output by the Pozyx
print(x_position_front)
#output the front tag's x-position
print(y_position_front)
#output the front tag's y-position
print(z_position_front)
#output the front tag's z-position
else: #otherwise, if the tag that is having its coordinates measured is the one connected to the external power source
global x_position_back #indicate to program that the global variable for the back tag's x-position is to be used
global y_position_back #indicate to program that the global variable for the back tag's y-position is to be used
global z_position_back #indicate to program that the global variable for the back tag's z-position is to be used
x_position_back = position.x
#set the x-position of the back tag to the x-value output by the Pozyx
y_position_back = position.y
#set the y-position of the back tag to the y-value output by the Pozyx
z_position_back = position.z
#set the z-position of the back tag to the z-value output by the Pozyx
print(x_position_back)
#output the back tag's x-position
print(y_position_back)
#output the back tag's y-position
print(z_position_back)
#output the back tag's z-position
#print("THE LOOP HAS BEEN EXITED"); FOR TESTING: print a statement that will allow us to see how exactly the loop
# running the main body is working and where it is at in its execution
'''NOTE: The following print statements were used previously to check if the global variables for x, y, and z
were being set to the Pozyx's outputs:
print(x_position);
print(y_position);
print(z_position);'''
def setAnchorsManual(self):
"""Adds the manually measured anchors to the Pozyx's device list one for one."""
for tag_id in self.tag_ids:
status = self.pozyx.clearDevices(tag_id)
for anchor in self.anchors:
status &= self.pozyx.addDevice(anchor, tag_id)
if len(anchors) > 4:
status &= self.pozyx.setSelectionOfAnchors(
PozyxConstants.ANCHOR_SELECT_AUTO, len(anchors))
# enable these if you want to save the configuration to the devices.
def printPublishConfigurationResult(self, status, tag_id):
"""Prints the configuration explicit result, prints and publishes error if one occurs"""
if tag_id is None:
tag_id = 0
if status == POZYX_SUCCESS:
print("Configuration of tag %s: success" % tag_id)
else:
self.printPublishErrorCode("configuration", tag_id)
def printPublishErrorCode(self, operation, network_id):
"""Prints the Pozyx's error and possibly sends it as a OSC packet"""
error_code = SingleRegister()
status = self.pozyx.getErrorCode(error_code, network_id)
if network_id is None:
network_id = 0
if status == POZYX_SUCCESS:
print("Error %s on ID %s, %s" %
(operation, "0x%0.4x" % network_id,
self.pozyx.getErrorMessage(error_code)))
if self.osc_udp_client is not None:
self.osc_udp_client.send_message(
"/error_%s" % operation, [network_id, error_code[0]])
else:
# should only happen when not being able to communicate with a remote Pozyx.
self.pozyx.getErrorCode(error_code)
print("Error % s, local error code %s" %
(operation, str(error_code)))
if self.osc_udp_client is not None:
self.osc_udp_client.send_message("/error_%s" % operation,
[0, error_code[0]])
def printPublishAnchorConfiguration(self):
for anchor in self.anchors:
print("ANCHOR,0x%0.4x,%s" %
(anchor.network_id, str(anchor.pos)))
if self.osc_udp_client is not None:
self.osc_udp_client.send_message("/anchor", [
anchor.network_id, anchor.pos.x, anchor.pos.y,
anchor.pos.z
])
sleep(0.025)
if __name__ == "__main__":
# Check for the latest PyPozyx version. Skip if this takes too long or is not needed by setting to False.
check_pypozyx_version = True
if check_pypozyx_version:
perform_latest_version_check()
# shortcut to not have to find out the port yourself.
serial_port = get_first_pozyx_serial_port()
if serial_port is None:
print("No Pozyx connected. Check your USB cable or your driver!")
quit()
# enable to send position data through OSC
use_processing = True
# configure if you want to route OSC to outside your localhost. Networking knowledge is required.
ip = "127.0.0.1"
network_port = 8888
# IDs of the tags to position, add None to position the local tag as well.
tag_ids = [None, 0x6728]
# necessary data for calibration
anchors = [
DeviceCoordinates(0x6e09, 1, Coordinates(0, 0, 0)),
DeviceCoordinates(0x674c, 1, Coordinates(1650, 0, 0)),
DeviceCoordinates(0x6729, 1, Coordinates(0, 480, 0)),
DeviceCoordinates(0x6765, 1, Coordinates(1650, 480, 0))
]
# positioning algorithm to use, other is PozyxConstants.POSITIONING_ALGORITHM_TRACKING
algorithm = PozyxConstants.POSITIONING_ALGORITHM_UWB_ONLY
# positioning dimension. Others are PozyxConstants.DIMENSION_2D, PozyxConstants.DIMENSION_2_5D
dimension = PozyxConstants.DIMENSION_2D
# height of device, required in 2.5D positioning
height = 1000
osc_udp_client = None
if use_processing:
osc_udp_client = SimpleUDPClient(ip, network_port)
pozyx = PozyxSerial(serial_port)
r = MultitagPositioning(pozyx, osc_udp_client, tag_ids, anchors,
algorithm, dimension, height)
r.setup()
while True:
r.loop()
print("THE FRONT X POSITION IS:" + str(x_position_front))
#used to see if the x-positions of the front tag were correct
print("THE FRONT Y POSITION IS:" + str(y_position_front))
#used to see if the y-positions of the front tag were correct
print("THE FRONT Z POSITION IS:" + str(z_position_front))
#used to see if the z-positions of the front tag were correct
print("THE BACK X POSITION IS:" + str(x_position_front))
#used to see if the x-positions of the back tag were correct
print("THE BACK Y POSITION IS:" + str(y_position_front))
#used to see if the y-positions of the back tag were correct
print("THE BACK Z POSITION IS:" + str(z_position_front))
#used to see if the z-positions of the back tag were correct
y = int(y_position_front)
x = int(x_position_front)
#break; #NOTE: this was previously used to end the execution of the Pozyx measuring to see if the new variables were working
#Previously used to check if the global variables for the x, y, and z coordinates were functioning properly:
#print(x_position);
#print(y_position);
#print(z_position);'''
import RPi.GPIO as GPIO #Pin setup for Entire Pi
import time
import curses #User Interface
import serial
#pin setup
GPIO.setmode(GPIO.BOARD)
GPIO.setup(13, GPIO.OUT)
GPIO.setup(22, GPIO.OUT)
GPIO.setup(15, GPIO.OUT)
GPIO.setup(18, GPIO.OUT)
#motor varibles
FR = GPIO.PWM(13,
50) #Front Right Motor #The value 50 is the Frequency
FL = GPIO.PWM(22, 50) #Front Left Motor #The value 12 is the GPIO pin
RR = GPIO.PWM(15, 50) #Rear Right Motor
RL = GPIO.PWM(18, 50) #Rear Left Motor
FR.start(100)
FL.start(100)
RR.start(100)
RL.start(100)
#curses setup
#screen = curses.initscr()
#curses.noecho()
#curses.cbreak()
#screen.keypad(True)
#User Interface
print('...Loading...')
while True:
#getUpdatedCoordinates()
print(y_position_front)
nicksThing()
if int(y_position_front) > 1400:
FR.ChangeDutyCycle(100)
FL.ChangeDutyCycle(100)
RR.ChangeDutyCycle(100)
RL.ChangeDutyCycle(100)
print('Almost there')
break
else:
print(y_position_front)
FR.ChangeDutyCycle(6.5)
FL.ChangeDutyCycle(8)
RR.ChangeDutyCycle(6.5)
RL.ChangeDutyCycle(8)
while True:
print("turning 90 degrees left")
FR.ChangeDutyCycle(5)
FL.ChangeDutyCycle(5)
RR.ChangeDutyCycle(5)
RL.ChangeDutyCycle(5)
time.sleep(.68)
FR.ChangeDutyCycle(100)
FL.ChangeDutyCycle(100)
RR.ChangeDutyCycle(100)
RL.ChangeDutyCycle(100)
break
while True:
#getUpdatedCoordinates()
print(x_position_front)
nicksThing()
if (x_position_front) > (1400):
FR.ChangeDutyCycle(100)
FL.ChangeDutyCycle(100)
RR.ChangeDutyCycle(100)
RL.ChangeDutyCycle(100)
print('Arrived')
break
else:
print(x_position_front)
FR.ChangeDutyCycle(6.5)
FL.ChangeDutyCycle(8)
RR.ChangeDutyCycle(6.5)
RL.ChangeDutyCycle(8)
print("tada!")
#cleanup
GPIO.cleanup
curses.nobreak()
screen.keypad(0)
curses.echo()
curses.endwin()
