def main():
# Open messageboard
mb = messageboard.MessageBoard("nav")
print(mb)
cmd = "directions"
def signal_handler(*args):
print("keyboard interrupt")
exit()
# set up signal handler to stop while loop
signal.signal(signal.SIGINT, signal_handler)
# Define default values
x_pix, y_pix = 250, 70
move_direction = "x"
move_magnitude = 0
floor = 2
while (True):
# first, read x_pos, y_pos, angle, and floor from Yash's tags
target = ["state", "updated_state"]
msg_list = mb.readMsg(target)
# print("1")
if len(msg_list) > 0:
msg = msg_list[-1] # get most recent one message
x_pix = msg["x_pos_pixels"] # FINAL
y_pix = msg["y_pos_pixels"] # FINAL
floor = msg["floor"] # FINAL
# communicate with server
url = "http://drjava.herokuapp.com/getinstruction"
payload = {"xpos": x_pix, "ypos": y_pix, "floor": floor}
# print("Getting Response")
response = requests.get(url, params=payload)
respDict = response.json()
# print(respDict)
mb.postMsg(cmd, respDict)
time.sleep(0.1)
def main():
port1 = PORT1
port2 = PORT2
if len(argv) > 1:
port1 = argv[1]
if len(argv) > 2:
port2 = argv[2]
# Open serial connections
ser1 = Serial(port=port1, baudrate=BAUDRATE, timeout=TIMEOUT)
if not ser1.is_open:
ser1.open()
print(ser1)
ser2 = Serial(port=port2, baudrate=BAUDRATE, timeout=TIMEOUT)
if not ser2.is_open:
ser2.open()
print(ser2)
# Open messageboard
mb = messageboard.MessageBoard("tag")
print(mb)
cmd = "location_data"
floor = 2
status = "testing"
sorted_data = []
points_list = []
# Get point and static measurement data
with open('point_data_filtered.pickle', 'rb') as infile1:
sorted_data = pickle.load(infile1)
with open('points_list.pickle', 'rb') as infile2:
points_list = pickle.load(infile2)
for i in range(len(points_list)):
point = points_list[i]
new_point = (int(point[0]), int(point[1]))
points_list[i] = new_point
name_to_point = NAME_TO_POINT
def signal_handler(*args):
print("keyboard interrupt")
ser1.close()
ser2.close()
exit()
# set up signal handler to stop while loop
signal.signal(signal.SIGINT, signal_handler)
clearSer(ser1)
clearSer(ser2)
# write les and send enter command to stop tag
# ser.write(CLEAR)
ser1.write(LES)
ser1.write(ENTER)
ser2.write(LES)
ser2.write(ENTER)
# clear starting nonsense
ser1.readline()
ser1.read(5)
ser2.readline()
ser2.read(5)
while (True):
now = time.strftime("%Y-%m-%d %H:%M:%S", time.gmtime())
# line1 = readLine(ser1)
# if len(line1) < 1:
# print("no line - 1")
# continue
# line2 = readLine(ser2)
# if len(line2) < 1:
# print("no line - 2")
# continue
# get x, y coords
# dists1 = line1
# anch_pred1, prediction1 = get_location(dists1)
# x1, y1 = prediction1
# dists2 = line2
# anch_pred2, prediction2 = get_location(dists2)
# x2, y2 = prediction2
# get ave position
x = 255
y = 75
x_m = PIX_TO_M_X * x
y_m = PIX_TO_M_Y * y
# get angle
angle = 0
# print((x, y, angle))
msg = {
"x_pix_global": x,
"y_pix_global": y,
"x_meter_global": x_m,
"y_meter_global": y_m,
"floor": floor,
"status": status,
"global_angle": angle
}
mb.postMsg(cmd, msg)
time.sleep(2)
ser.close()
# 0 - not found
# 1 - X centered
# 2 - Y centered
# 3 - move left
# 4 - move right
# 5 - move up
# 6 - move down
# 7 - target lost
import random
import skynet
import messageboard
arm = skynet.robotArm()
arm.ready()
mb = messageboard.MessageBoard('arm')
baseRot = 20
repeatAngleX = 10
repeatAngleY = 10
angleDeltaX = 2
angleDeltaY = 2
previousXDir = -1
previousYDir = -1
previousMoved = None
targetLossThresh = 3
targetsLost = 0
def main():
# Open messageboard
mb = messageboard.MessageBoard("state")
print(mb)
cmd = "updated_state"
def signal_handler(*args):
print("keyboard interrupt")
exit()
# set up signal handler to stop while loop
signal.signal(signal.SIGINT, signal_handler)
x_meters, y_meters = 0, 0
x_pix, y_pix = 250, 70
orientation = 0 # 0 = x, 1 = y, 2 = -x, 3 = -y
orientation_angle = 0 # angle
status = "dead" # delivering / arrived / waiting / idle / dead
angle = 0.0 # if < 0, then ignore
move_direction = "x"
move_magnitude = 0
floor = 2
while(True):
# first, read x_pos, y_pos, angle, and floor from Yash's tags
target = ["tag", "location_data"]
msg_list = mb.readMsg(target)
if len(msg_list) > 0:
msg = msg_list[-1] # get most recent one message
x_pix = msg["x_pix_global"] # FINAL
y_pix = msg["y_pix_global"] # FINAL
orientation_angle = msg["global_angle"]
floor = msg["floor"] # FINAL
meters = pixels_to_meters(x_pix, y_pix)
x_meters = meters[0] # FINAL
y_meters = meters[1] # FINAL
if math.fabs(orientation_angle) >= 0 and math.fabs(orientation_angle) <= 45:
orientation = 0
elif math.fabs(orientation_angle) >= 135 and math.fabs(orientation_angle) <= 180:
orientation = 2
elif 45 < orientation_angle and orientation_angle < 135:
orientation = 1
else: # -135 < orientation_angle < -45
orientation = 3 # FINAL
# second, read corrected angle from Khanh's "angle" module
target = ["angle", "angle_correction"]
msg_list = mb.readMsg(target)
if len(msg_list) > 0:
msg = msg_list[-1] # get most recent one message
angle = msg["angle"] # FINAL
# NOTE TO COLLINS: this angle is different from Yash's because it's more
# "fine-grained" and accurate than Yash's, which is just used to infer
# crude orientation. This angle will be = -1 if my sensors know they're
# not in a good place to get a good angle measurement, and will be a
# positive angle less than 90 degrees if I'm in a good place to measure.
# Based on this, you can infer how to "correct" the angle.
# third, read move direction and magnitude from Yash's "nav" module
target = ["nav", "directions"]
msg_list = mb.readMsg(target)
if len(msg_list) > 0:
msg = msg_list[-1] # get most recent one message
move_direction = msg["move_direction"] # FINAL
move_magnitude = msg["move_magnitude"] # FINAL
# last, update "status" appropriately
if status == "delivering":
if move_magnitude <= 20: # 20 cm? what unit is "nav" in?
status = "arrived" # FINAL
elif status == "arrived":
# check Khanh's "door" module to see if at door yet
target = ["door", "location_correction"]
msg_list = mb.readMsg(target)
if len(msg_list) > 0:
msg = msg_list[-1] # get last known status
if msg["at_door"]:
status = "waiting"
# NOTE: "arrived" means near the office door, but not entirely sure
# if at door. From "door" module, "waiting" means stopped at front of door.
elif status == "waiting":
time.sleep(300) # wait 5 min ????? TODO: how to do this exactly? cuz rn, it just pauses the program and doens't post to MB for 5min
status = "idle"
msg = {
"x_pos_meters": x_meters,
"y_pos_meters": y_meters,
"x_pos_pixels": x_pix,
"y_pos_pixels": y_pix,
"orientation": orientation,
"angle": angle,
"status": status,
"move_direction": move_direction,
"move_magnitude": move_magnitude,
"floor": floor
}
mb.postMsg(cmd, msg)
time.sleep(0.1)
import sys
import cv2
import time
import tracker
import messageboard
# create messageboard for button tracker
# can get messages since creation of MB
# or since last read
mb = messageboard.MessageBoard('tracker')
waitForArm = False
targetReceived = False
target = None
state1 = None
state2 = None
state = None
vis = True
# create tracker
tracker = tracker.tracker(visualize=vis)
while(True):
if not targetReceived:
# check messageboard once every second
# time.sleep(1)
# check messageboard for target
targetList = mb.readMsg(['nav', 'directions'])
import ksensors
import time
import messageboard
k = ksensors.ksensors()
state = False # False = STOP!; True = GO
mb = messageboard.MessageBoard("collision")
# print(str(mb))
def goodPosition(x, y):
# area1 =
# bottom left : 108, 85px
# top right : 132, 67px
# area2 =
# bottom left: 180, 85px
# top right: 206, 68px
# area3 =
# bottom left: 251, 85px
# top right: 281, 67px
if y >= 67 and y <= 85:
if x >= 108 and x <= 132: # area1
return True
else if x >= 180 and x <= 206: # area2
return True
else if x >= 251 and x <= 281:
return True
return False
while True:
import sys
import messageboard
mb = messageboard.MessageBoard('nav')
while (True):
print('Enter target button, 1000 to exit')
button = int(input())
armStateList = mb.readMsg(['arm', 'arm_state'])
print(len(armStateList))
if button == 1000:
mb.postMsg('directions', {'arm_req': False, 'floor_to': button})
break
else:
mb.postMsg('directions', {'arm_req': True, 'floor_to': button})
sys.exit()