fieldcontrol_test

In [1]:

    

# Cross-notebook include shim
with open("nbinclude.ipynb") as nbinclude_f: # don't rename nbinclude_f
    import IPython.nbformat.current
    get_ipython().run_cell(IPython.nbformat.current.read(nbinclude_f, 'json').worksheets[0].cells[0].input)
nbinclude('netrobot')
nbinclude('simplekernel')
import time, lcm
from python.forseti2 import piemos_cmd
import IPython.display
import sys
import threading


    

In [2]:

    

r=NetRobot()


    

In [3]:

    

r.cmd(1,1)


    

In [4]:

    

r.cmd(1,0)


    

In [5]:

    

r.cmd(0,1)


    

In [6]:

    

r.cmd(0,0)


    

In [7]:

    

class RobotRunner:
    def __init__(self, robot):
        self.robot=robot
        
    def cmd_handler(self, channel, data):
        msg=piemos_cmd.decode(data)
#         IPython.display.clear_output()
#         print('forseti2.piemos_cmd:'
#             + 'channel='
#             + str(channel)
#             + ', header.seq='
#             + str(msg.header.seq)
#             + ', header.time='
#             + str(msg.header.time)
#             + ', auton=' 
#             + str(msg.auton)
#             + ', enabled=' 
#             + str(msg.enabled)
#             + ', is_blue=' 
#             + str(msg.is_blue)
#             + ', game_time=' 
#             + str(msg.game_time))
        if msg.enabled:
            self.robot.enable()
        else:
            self.robot.disable()
#         sys.stdout.flush()
        
    def lcm_run(self):
        lc=lcm.LCM('udpm://239.255.76.67:7667?ttl=1')
        sub=lc.subscribe("piemos0/cmd", self.cmd_handler)
        while True:
            completed=False
            try:
                lc.handle()
                completed=True
            except KeyboardInterrupt as e:
                print e
            yield completed


    

In [8]:

    

xs=np.zeros(shape=[100], dtype=np.float32)


    

In [9]:

    

times=np.zeros(shape=[100], dtype=np.float32)


    

In [10]:

    

import numpy as np
def go():
    while True:
        cmds=np.arange(-.5, .5, .05)
        cmds1=np.arange(.5, -.5, -.05)
        for cmd in cmds:
            r.cmd(cmd, -cmd)
            yield
        for cmd in cmds1:
            r.cmd(cmd, -cmd)
            yield
def modulate():
    while True:
        r.disable()
        yield
        r.enable()
        yield
def time_test():
    i=0
    last_time=time.time()
    while True:
        now=time.time()
        
        xs[i]=i
        times[i]=now - last_time
        i +=1
        last_time=now
        print i
        if i >= 100:
            yield False
        yield


    

In [11]:

    

m1=modulate()
g1=go()
tt=time_test()


    

In [12]:

    

rr=RobotRunner(r)
lcm_run_gen=rr.lcm_run()


    

In [13]:

    

r.enable()


    

In [14]:

    

next(g1)


    

In [15]:

    

next(lcm_run_gen)


    

    
Out[15]:





True

In [16]:

    

sk=SimpleKernel()


    

In [17]:

    

# sk.add_coroutine(lcm_run_gen, .01)


    

In [18]:

    

# sk.add_coroutine(g1, .1)


    

In [19]:

    

sk.add_coroutine(tt, .01)


    

In [20]:

    

# sk.add_coroutine(m1, .01)


    

In [21]:

    

sk.tick()


    

    




1






    
Out[21]:





True

In [22]:

    

# next(lcm_run_gen)


    

In [23]:

    

sk.run()


    

    




2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100

In [24]:

    

%matplotlib inline


    

In [25]:

    

import matplotlib.pyplot as plt


    

In [26]:

    

plt.plot(xs[14:],times[14:])


    

    
Out[26]:





[<matplotlib.lines.Line2D at 0x1371070>]






    

In [27]:

    

fft_times = np.fft.fft(times)


    

In [28]:

    

n=len(fft_times)


    

In [29]:

    

plt.plot(np.abs(fft_times))


    

    
Out[29]:





[<matplotlib.lines.Line2D at 0x1817af0>]






    

In [29]: