Exemplo n.º 1
0
def main():

	#List of hostnames of robots used
	lab.robots = ["alpha1"]
	lab.init("cricket")

	#Use crickets in chirp mode
	cricketList = cricketScan()
	crickets = buildDictionary(cricketList, 'id')
	for x in crickets:
		crickets[x].setMode(MODE_LISTEN)

	#Use gamepad
	gamepad = lab.Joystick()

	dt = 0.2

	lab.robot["alpha1"].set("goFlag", 1)

	while True:

		time_start = time.clock()

		#Get control input from gamepad & send to robot
		axes = gamepad.axes()
		(v, r) = lab.axes2vr(axes)
		lab.robot["alpha1"].robot.DriveVR(v, r)
		
		
		#node = random.randrange(1,11)
		node = 4
		node_name = "beac_" + str(node)

		#Get measurement
		(nn, dist) = crickets[node_name].listen()
		for c in crickets:
			crickets[c].s.flush()
		print nn, node, dist
		dist = int(dist)*10

		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()
Exemplo n.º 2
0
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.2
    m = EKFModel(4)

    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

    m.f = const_velocity
    m.Q = eye(2) * 1000
    m.R = eye(10) * 20
    m.P = eye(4) * 100

    # 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)

        # Wait for valid camera readings

    cx = 0
    cy = 0
    if CAMERA:
        while cx == 0:
            [cx, cy, cb, valid] = camera.getxy()
    m.x = array([cx, cy, 0, 0]).T
    lab.robot["alpha1"].set("goFlag", 1)

    plot = lab.Plot()
    plot.axes((-2000, 2000), (-2000, 2000))

    cx_old = 0
    cy_old = 0

    data = []
    k = 0

    while True:

        k += 1
        t = k * dt

        time_start = time.clock()
        [mx_old, my_old] = [m.x[0], m.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 = 4.0
        # v = 250
        # w = -2.405*sin(t/scale)/scale
        # (vl, vr) = lab.vw2lr(v,w)
        # lab.robot["alpha1"].robot.DriveLR(vl, vr)

        # 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()
        for c in crickets:
            crickets[c].s.flush()
        print nn, node, dist
        dist = int(dist) * 10

        def hn(x):
            return h(x, node - 1)

        m.h = hn

        m.predict([0, 0])
        if dist > -1:
            m.updateSerial(dist, node - 1)

        # Get camera reading and plot if valid
        if CAMERA:
            [cx, cy, cb, valid] = camera.getxy()
            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
        plot.point(m.x[0], m.x[1], "red")
        plot.line(mx_old, my_old, m.x[0], m.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)
Exemplo n.º 3
0
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()
Exemplo n.º 4
0
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)