def main():
#List of hostnames of robots used
lab.robots = ["alpha1"]
lab.init("test-setup")
#Use crickets in chirp mode
cricketList = cricketScan()
crickets = buildDictionary(cricketList, 'id')
for x in crickets:
crickets[x].setMode(MODE_CHIRP)
#Use camera
camera = lab.Camera()
camera.connect()
#Use gamepad
gamepad = lab.Joystick()
#crickets["beac_8"].setMode(MODE_LISTEN)
#crickets["beac_2"].setMode(MODE_CHIRP)
#crickets["beac_2"].chirp()
#time.sleep(0.1)
#print crickets["beac_8"].listen()
#print "break"
#crickets["beac_2"].chirp()
#time.sleep(0.1)
#print crickets["beac_8"].listen()
#Wait for valid camera readings
cx = 0
while cx == 0:
[cx, cy, cb, valid] = camera.getxy()
lab.robot["alpha1"].set("updateX", array([cx, cy, cb]).T)
lab.robot["alpha1"].set("goFlag", 1)
print "go"
plot = lab.Plot()
plot.axes((-1000,1000),(-1000,1000))
cx_old = 0
cy_old = 0
x = array([0,0,0]).T
#time_start = time.clock()
while True:
time_start = time.clock()
#Get camera reading
[cx,cy,cb,valid] = camera.getxy()
#Plot it if valid
if valid:
#pyplot.plot([cx_old, cx], [cy_old, cy], 'bx-')
plot.line(cx_old, cy_old, cx, cy, "blue")
[cx_old, cy_old] = [cx, cy]
#Get control input from gamepad, convert to velocity & send to robot
axes = gamepad.axes()
(vt, w) = lab.axes2vw(axes)
(vl, vr) = lab.vw2lr(vt, w)
lab.robot["alpha1"].set("v", vt)
lab.robot["alpha1"].set("w", w)
lab.robot["alpha1"].robot.DriveLR(vl, vr)
#Get estimated state from robot & plot
[mx_old, my_old] = [x[0], x[1]]
x = lab.robot["alpha1"].get("x")
#pyplot.plot([mx_old, x[0]], [my_old, x[1]], 'rx-')
plot.line(mx_old, my_old, x[0], x[1], "red")
if msvcrt.kbhit(): key = msvcrt.getch()
else: key = ""
if key == "q": break
#Update plot
#pyplot.draw()
time_pad = time.clock() - time_start
print time_pad
lab.robot["alpha1"].robot.Stop()
lab.deinit()
def main():
CAMERA = True
# List of hostnames of robots used
lab.robots = ["alpha1"]
lab.init("sched-trace")
# Use crickets in chirp mode
cricketList = cricketScan()
crickets = buildDictionary(cricketList, "id")
for x in crickets:
crickets[x].setMode(MODE_LISTEN)
# Use camera
if CAMERA:
camera = lab.Camera()
camera.connect()
# Use gamepad
gamepad = lab.Joystick()
# State-space model
dt = 0.1
def const_velocity(x, u):
return array(
[
x[0] + x[2] * dt + 0.5 * dt * dt * u[0],
x[1] + x[3] * dt + 0.5 * dt * dt * u[1],
x[2] + u[0] * dt,
x[3] + u[1] * dt,
]
).T
# SS model for simulation
ms = EKFModel(3)
ms.dt = dt
# Cricket locations and measurement equation
node_xy = (
(-1810, -2080),
(-1810, 0),
(-1810, 2070),
(-890, 3220),
(680, 3220),
(1630, 2040),
(1630, -200),
(1630, -2090),
(740, -3250),
(-870, -3250),
)
node_height = 1230
def h(x, i):
return sqrt((x[0] - node_xy[i][0]) ** 2 + (x[1] - node_xy[i][1]) ** 2 + node_height ** 2)
q0 = 0.2
x0 = lambda: array([random.gauss(0, q0), random.gauss(0, q0), 0, 0])
q = 500
r = 50
process_noise = lambda: array([random.gauss(0, q), random.gauss(0, q)])
sensor_likelihood = lambda y, x, i: normpdf(
sqrt((x[0] - node_xy[i][0]) ** 2 + (x[1] - node_xy[i][1]) ** 2 + node_height ** 2) - y, 0, r
)
pf = ParticleFilter(500, x0, const_velocity, process_noise)
# Wait for valid camera readings
cx = 0
cy = 0
if CAMERA:
while cx == 0:
[cx, cy, cb, valid] = camera.getxy()
lab.robot["alpha1"].set("goFlag", 1)
plot = lab.Plot()
plot.axes((-2000, 2000), (-2000, 2000))
cx_old = 0
cy_old = 0
x = [0, 0, 0, 0]
data = []
k = 0
while True:
k += 1
t = k * dt
time_start = time.clock()
[mx_old, my_old] = [x[0], x[1]]
data.append([mx_old, my_old, cx_old, cy_old])
# Get control input from gamepad & send to robot
axes = gamepad.axes()
# (v, r) = lab.axes2vr(axes)
# lab.robot["alpha1"].robot.DriveVR(v, r)
scale = 3.0
v = 250
w = -2.405 * sin(t / scale) / scale
(vl, vr) = lab.vw2lr(v, w)
lab.robot["alpha1"].robot.DriveLR(vl, vr)
# ms.predict([v,w])
# Schedule
# mintrace = 1e6
# for j in range(1,11):
# def hj(x):
# return h(x, j-1)
# hx = hj(m.x)
# if hx > 30:
# #R = m.R[j-1,j-1] + hx
# H = m.csdJacobian(hj, m.x)
# PHT = m.P*(H.T)
# S = H*PHT + m.R[j-1,j-1]
# K = PHT / S
# Pj = (eye(4) - K*H)*m.P
# if trace(Pj) < mintrace:
# mintrace = trace(Pj)
# node = j
node = random.randrange(1, 11)
node_name = "beac_" + str(node)
# Get measurement
(nn, dist) = crickets[node_name].listen()
# dist = (h(ms.x, node-1) + random.gauss(0, 0.05))
for c in crickets:
crickets[c].s.flush()
dist = float(dist) * 10
print node, dist
# if dist > -1: m.updateSerial(dist, node-1)
p_yx = lambda y, x: sensor_likelihood(y, x, node - 1)
if dist > -1:
x = pf.predict_update(dist, p_yx)
# Get camera reading and plot if valid
if CAMERA:
[cx, cy, cb, valid] = camera.getxy()
# [cx,cy,valid] = [ms.x[0], ms.x[1], True]
if valid:
# cy += 100
# cx -= 100
plot.line(cx_old, cy_old, cx, cy, "blue")
[cx_old, cy_old] = [cx, cy]
# Get estimated state & plot
# for i in range(pf.N):
plot.point(x[0], x[1], "red")
# plot.point(pf.x[i][0], pf.x[i][1], "red")
plot.line(mx_old, my_old, x[0], x[1], "red")
if msvcrt.kbhit():
key = msvcrt.getch()
else:
key = ""
if key == "q":
break
loop_time = time.clock() - time_start
# print loop_time, loop_time < dt, t
if loop_time < dt:
time.sleep(dt - loop_time)
lab.robot["alpha1"].robot.Stop()
lab.deinit()
writer = csv.writer(open("C:/data.csv", "w"))
for row in data:
writer.writerow(row)