base_lcm_driver

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)


    

In [2]:

    

import time, lcm
from python.forseti2 import piemos_cmd
from python.fearing import base_cmd, reflectance_scan
import python.fearing
import threading
import sys


    

In [3]:

    

from python.mbedrpc import *
import threading

class Motor:
    def __init__(self, a1, a2):
        self.a1=a1
        self.a2=a2
    def cmd(self, speed):
        if speed >=0:
            self.a1.write(speed)
            self.a2.write(0)
        else:
            self.a1.write(0)
            self.a2.write(-speed)
class Robot:
    def __init__(self, dev='/dev/ttyACM0'):
        self.mbed=SerialRPC(dev, 115200)
        a1=PwmOut(self.mbed, p21)
        a2=PwmOut(self.mbed, p22)
        b1=PwmOut(self.mbed, p23)
        b2=PwmOut(self.mbed, p24)
        self.m_right = Motor(a1, a2)
        self.m_left = Motor(b1, b2)
        self.enabled=True
        self.last_left=0
        self.last_right=0
        self.sensors=[]
        for i in (p20,p19,p18,p17,p16,p15):
            self.sensors.append(AnalogIn(self.mbed, i))
        self.rlock=threading.Lock()
    def enable(self):
        self.rlock.acquire()
        self.enabled=True
        self._cmd(self.last_left, self.last_right)
        self.rlock.release()
    def disable(self):
        self.rlock.acquire()
        self.enabled=False
        self._cmd(self.last_left, self.last_right)
        self.rlock.release()
    def drive(self, left, right):
        self.rlock.acquire()
        self._cmd(left, right)
        self.rlock.release()
    def cmd(self, left, right):
        self.rlock.acquire()
        self._cmd(left, right)
        self.rlock.release()
    def _cmd(self, left, right):
        self.last_left=left
        self.last_right=right
        if self.enabled:
            self.m_left.cmd(-left)
            self.m_right.cmd(right)
        else:
            self.m_left.cmd(0)
            self.m_right.cmd(0)
    def read_sensors(self):
        """ returns an array of the line sensor reflectance values
        """
        self.rlock.acquire()
        def read(sensor): return sensor.read()
        retu=map(read, self.sensors)
        self.rlock.release()
        return retu


    

In [4]:

    

class Base_LCM_Driver:
    def __init__(self, robot):
        self.robot=robot
        self.lcm = lcm.LCM('udpm://239.255.76.67:7667?ttl=1')
        sub = self.lcm.subscribe("robot0/cmd", self.handle_base_cmd)
        
        self.running = False
        self.msg = reflectance_scan()
        self.msg.header = python.fearing.header()
        
    def handle_base_cmd(self, chan, data):
        msg = base_cmd.decode(data)
#         print 'handle_base_cmd', msg.left_cmd, msg.right_cmd
#         sys.stdout.flush()
        self.robot.cmd(msg.left_cmd, msg.right_cmd)
        
    def transmit_start(self):
        t=threading.Thread(target=self.transmit_run)
        t.daemon = True
        t.start()
    
    def transmit_run(self):
        self.running = True
        while self.running:
            self.transmit_state()
#             time.sleep(.01)
        
    def transmit_state(self):
        self.msg.reflectaces = self.robot.read_sensors()
        self.lcm.publish('robot0/state', self.msg.encode())
#         self.robot.cmd(l,r)

#     def handle_joy(self, chan, data):
#         msg = xbox_joystick_state.decode(data)
#         l,r = msg.axes[1], msg.axes[4]
#         self.robot.cmd(l,r)
        
#     def handle_piemos(self, channel, data):
#         msg=piemos_cmd.decode(data)
#         if msg.enabled:
#             self.robot.enable()
#         else:
#             self.robot.disable()

    def lcm_run(self):
#         sub2=lc.subscribe("piemos0/cmd", self.handle_piemos)
        while True:
            self.lcm.handle()
            time.sleep(.01)


    

In [5]:

    

r0=Robot('/dev/ttyACM0')


    

In [6]:

    

r0.cmd(.1, .1)


    

In [7]:

    

r0.cmd(0,0)


    

In [8]:

    

bld=Base_LCM_Driver(r0)


    

In [9]:

    

# bld.transmit_run()


    

In [10]:

    

bld.transmit_start()


    

In [ ]:

    

bld.lcm_run()


    

In [11]:

    

bld.running=False


    

In [ ]: