def listen(self):
c = self.stdscr.getch()
#self.stdscr.addstr(str(c))
if c == 9:
self.tab_pressed()
else:
self.clear_flags()
if c >= 33 and c <= 126:
self.stdscr.addch(c)
self.current_command_str += chr(c)
self.current_word += chr(c)
elif c == 3:
#terminate
curses.nobreak(); stdscr.keypad(0); curses.echo(); curses.endwin()
sys.exit(0)
elif c == 10:
self.enter_pressed()
elif c == 127:
self.stdscr.addstr('\r>')
for i in range(0,len(self.current_command_str)):
self.stdscr.addch(' ')
self.current_command_str = self.current_command_str[:-1]
self.current_word = self.current_word[:-1]
self.stdscr.addstr('\r>%s' % self.current_command_str)
elif c == 27:
#debug key
self.stdscr.addstr('\n' + self.current_command_str)
self.stdscr.addstr('\n' + self.current_word)
print self.tab_completion_list
elif c == 32:
self.space_pressed()
def getXTargetAxis():
while True:
#getUpdatedCoordinates()
print(x_position_front)
if (x_position_front) > (8400):
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()
def main(screen):
screen = curses.initscr()
curses.noecho()
curses.cbreak()
screen.keypad(True)
screen.addstr("Hello World!!!")
screen.addstr("PRESS THE ARROW KEYS TO MOVE THE CAR")
while True:
c = screen.getch()
if c == curses.KEY_LEFT:
screen.addstr(5, 10, 'left key pressed')
Anticlockwise()
screen.refresh()
elif c == curses.KEY_RIGHT:
screen.addstr(5, 50, 'right key pressed')
Clockwise()
screen.refresh()
elif c == curses.KEY_UP:
screen.addstr(2, 30, 'UP key pressed')
Forward()
screen.refresh()
elif c == curses.KEY_DOWN:
screen.addstr(9, 30, 'Down key pressed')
Back()
screen.refresh()
curses.nobreak()
screen.keypad(False) # Enable keypad Mode
curses.echo()
curses.endwin()
def controller():
#Initialize ROS stuff
pub = rospy.Publisher('keyboard', Int32, queue_size=1) #publish to the keyboard topic
rospy.init_node('controller') #name this node controller
rate = rospy.Rate(10) #10hz
#Initialize curses environment
stdscr = curses.initscr()
curses.noecho()
curses.cbreak()
stdscr.keypad(1)
stdscr.nodelay(1)
direction = 0
while not rospy.is_shutdown():
c = stdscr.getch()
#0 = No action. 1 = forwards. 2 = left. 3 = backwards. 4 = right. 5 for break. Press q to quit
if c == ord('w') or c == curses.KEY_UP:
direction = 1
elif c == ord('a') or c == curses.KEY_LEFT:
direction = 2
elif c == ord('s') or c == curses.KEY_UP:
direction = 3
elif c == ord('d') or c == curses.KEY_DOWN:
direction = 4
elif c == ord(' '):
direction = 5
elif c == ord('q'):
break;
else:
direction = 0
#curses has a delay before registering a key as being held. Not sure best way to handle it
curses.flushinp()
pub.publish(direction)
rate.sleep()
#Close curses environment
curses.nobreak()
stdscr.keypad(0)
curses.echo()
curses.endwin()
FR.ChangeDutyCycle(100)
FL.ChangeDutyCycle(100)
RR.ChangeDutyCycle(100)
RL.ChangeDutyCycle(100)
break
while True:
#getUpdatedCoordinates()
print(x_position_front)
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()
def closeCurses(stdscr):
stdscr.keypad(0)
curses.nobreak()
curses.echo()
curses.endwin()
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()
def getUpdatedCoordinates():
# 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)
if int(y_position_front) > 1400:
FR.ChangeDutyCycle(100)
FL.ChangeDutyCycle(100)
RR.ChangeDutyCycle(100)
RL.ChangeDutyCycle(100)
print('Almost there')
break
else:
#getUpdatedCoordinates()
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)
if (x_position_front) > (1400):
FR.ChangeDutyCycle(100)
FL.ChangeDutyCycle(100)
RR.ChangeDutyCycle(100)
RL.ChangeDutyCycle(100)
print('Arrived')
break
else:
#getUpdatedCoordinates()
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()
GPIO.output(33, False)
GPIO.output(11, True)
GPIO.output(13, False)
GPIO.output(15, True)
elif char == curses.KEY_RIGHT:
print("Right")
GPIO.output(33, True)
GPIO.output(11, False)
GPIO.output(13, False)
GPIO.output(15, True)
elif char == curses.KEY_LEFT:
print("Left")
GPIO.output(33, False)
GPIO.output(11, True)
GPIO.output(13, True)
GPIO.output(15, False)
elif char == 10:
print("Stop")
GPIO.output(33, False)
GPIO.output(11, False)
GPIO.output(13, False)
GPIO.output(15, False)
finally:
#Close down curses properly, inc turn echo back on!
curses.nobreak(); screen.keypad(0); curses.echo()
curses.endwin()
