Пример #1
0
def testNewSDO(filename):
    from can import ReplyLog
    can = CAN(ReplyLog(filename))
    can.resetModules()
    (nodeID, dataIndex, subIndex) = 10, 16 * 256 + 10, 0
    reader = ReadSDO(nodeID, dataIndex, subIndex)
    for packet in reader.generator():
        if packet != None:
            can.sendData(*packet)
        reader.update(can.readPacket())
    print("RESUT DATA:", reader.result)
Пример #2
0
def testNewSDO( filename ):
  from can import ReplyLog
  can=CAN(ReplyLog(filename))
  can.resetModules()
  ( nodeID, dataIndex, subIndex ) = 10, 16*256+10, 0
  reader = ReadSDO( nodeID, dataIndex, subIndex )
  for packet in reader.generator():
    if packet != None:
      can.sendData( *packet )
    reader.update( can.readPacket() )
  print("RESUT DATA:", reader.result)
Пример #3
0
class Spider3(object):
    UPDATE_TIME_FREQUENCY = 20.0  # Hz 

    # status word
    EMERGENCY_STOP = 0x0001
    DEVICE_READY = 0x0002  # also releaseState
    MASK_HEARTBEAT = 0x8000

    def __init__(self, can=None):
        if can is None:
            self.can = CAN()
#            self.can.resetModules()
        else:
            self.can = can
        self.time = 0.0
        self.status_word = None  # not defined yet
        self.wheel_angles = [None]*4  # original CAN data
        self.drive_status = [None]*4
        self.zero_steering = None  # zero position of all 4 wheels
        self.alive = 0  # togle with 128
        self.speed_cmd = [0, 0]
        self.status_cmd = 3
        self.extensions = []
        self.data_sources = []
        self.threads = []
        self.modulesForRestart = []
        self.led = 0
        self.led_time = 0.0
#        self.can.sendOperationMode()
  
    def __del__(self):
        pass
#        self.can.sendPreoperationMode() 

    # TODO to move out, can.py??
    def check_modules(self, packet):
        # test if all modules are in Operation mode, if not restart them
        id, data = packet
        if id & 0xFF0 == 0x700: # heart beat message
            moduleId = id & 0xF
            assert( len(data) == 1 )
            if data[0] != 5:
                self.can.printPacket( id, data )
                if not moduleId in self.modulesForRestart:
                    self.modulesForRestart.append( moduleId )
                    print("RESET", moduleId)
                    self.can.sendData( 0, [129,moduleId] )
#                if moduleId in [0x01, 0x02]:
#                    if (0x180 | moduleId) in self.encData:
#                      # The encoder information is invalid during a reset
#                      del self.encData[0x180 | moduleId]
            elif data[0] == 127: # restarted and in preoperation
                print("SWITCH TO OPERATION", moduleId)
                self.can.sendData( 0, [1,moduleId] ) 
            elif moduleId in self.modulesForRestart:
                print("RUNNING", moduleId)
                self.modulesForRestart.remove(moduleId)

    def register_data_source(self, name, function, extension=None):
        self.data_sources.append((name, function))
        if extension is not None:
            self.extensions.append((name, extension))

    def wait(self, duration):
        start_time = self.time
        while self.time - start_time < duration:
            self.update()

    def update_status_word(self, packet):
        msg_id, data = packet
        if msg_id == 0x200:
            assert len(data) == 2, packet
            prev = self.status_word
            self.status_word = struct.unpack('H', bytearray(data))[0]
            if prev is not None:
                diff = prev ^ self.status_word
                is_alive = diff & self.MASK_HEARTBEAT
                if is_alive == 0:
                    # make sure nothing is missing (for now)
                    print("\n!!!Spider is DEAD!!! (or data are missing)\n")
                if (diff & 0x7FFF) != 0:
                    print("Status %d -> %d" % 
                            (prev & 0x7FFF, self.status_word & 0x7FFF))

        elif msg_id == 0x201:
            assert len(data) == 8, packet
            prev = self.wheel_angles
            self.wheel_angles = struct.unpack('HHHH', bytearray(data))
            if prev != self.wheel_angles and self.zero_steering is not None:
                # note, that self.zero_steering has 200ms update rate,
                # i.e. it does not have to be immediately defined
                print('Wheels: %.2f' % self.time, [fix_range(a - b) for a, b in zip(self.wheel_angles, self.zero_steering)])

        elif msg_id == 0x202:
            assert len(data) == 8, packet  # drive status + zero positions
            self.drive_status = struct.unpack('HHHH', bytearray(data))
            drive = self.drive_status[0] - self.drive_status[2], self.drive_status[3] - self.drive_status[1]
            if abs(drive[0]) + abs(drive[1]) > 10:
                print('Drive:', drive, abs(drive[0]) + abs(drive[1]))

        elif msg_id == 0x203:
            assert len(data) == 8, packet
            prev = self.zero_steering
            self.zero_steering = struct.unpack('HHHH', bytearray(data))
            # make sure that calibration did not change during program run
            assert prev is None or prev == self.zero_steering, (prev, self.zero_steering)

    def send_speed(self):
        self.can.sendData(0x400, [self.status_cmd, self.alive])
        self.can.sendData(0x401, self.speed_cmd)  # drive control data drive, steering
        self.alive = 128 - self.alive

    def send_LED(self):
        self.can.sendData(0x410, [1, 2, 3, 4, 5, 6, 7, self.led])
        if self.time - self.led_time > 0.5:
            self.led = 1 - self.led
            self.led_time = self.time

    def set_raw_speed(self, fwd_rev_drive, left_right):
        print('set_raw_speed', fwd_rev_drive, left_right)
        self.speed_cmd = [fwd_rev_drive, left_right]

    def update(self):
        while True:
            packet = self.can.readPacket()
            self.check_modules(packet)
            self.update_status_word(packet)
            for (name,e) in self.extensions:
                e(self, packet[0], packet[1])
            
            # make sure that all updates get also termination SYNC (0x80)
            # there is no SYNC on Spider3 yet, use 0x200 for now
            if packet[0] == 0x200:
                break
        
        # send data related to other sources
        for (id,fce) in self.data_sources:
            data = fce()
            if data != None:
                for (name,e) in self.extensions:
                    e(self, id, data)

        self.time += 1.0/self.UPDATE_TIME_FREQUENCY  
        self.send_speed()
        self.send_LED()

    def stop(self):
        "send stop command and make sure robot really stops"
        self.set_raw_speed(0, 0)
        for i in range(10):
            self.update()
Пример #4
0
class JohnDeere(object):
    UPDATE_TIME_FREQUENCY = 5.0  #20.0  # Hz

    def __init__(self, can=None):
        if can is None:
            self.can = CAN()
            self.can.resetModules()
        else:
            self.can = can
        self.canproxy = CANProxy(self.can)
        self.time = 0.0
        self.gas = None
        self.steering_angle = 0.0  # in the future None and auto-detect
        self.buttonGo = None
        self.desired_speed = 0.0
        self.filteredGas = None
        self.compass = None
        self.compassRaw = None
        self.compassAcc = None
        self.compassAccRaw = None
        self.drop_ball = False  # TODO move to ro.py only
        self.extensions = []
        self.data_sources = []
        self.modulesForRestart = []
        self.can.sendOperationMode()

    def __del__(self):
        self.can.sendPreoperationMode()

    # TODO to move out, can.py??
    def check_modules(self, packet):
        # test if all modules are in Operation mode, if not restart them
        id, data = packet
        if id & 0xFF0 == 0x700:  # heart beat message
            moduleId = id & 0xF
            assert (len(data) == 1)
            if data[0] != 5:
                self.can.printPacket(id, data)
                if not moduleId in self.modulesForRestart:
                    self.modulesForRestart.append(moduleId)
                    print "RESET", moduleId
                    self.can.sendData(0, [129, moduleId])
#                if moduleId in [0x01, 0x02]:
#                    if (0x180 | moduleId) in self.encData:
#                      # The encoder information is invalid during a reset
#                      del self.encData[0x180 | moduleId]
            elif data[0] == 127:  # restarted and in preoperation
                print "SWITCH TO OPERATION", moduleId
                self.can.sendData(0, [1, moduleId])
            elif moduleId in self.modulesForRestart:
                print "RUNNING", moduleId
                self.modulesForRestart.remove(moduleId)

    def register_data_source(self, name, function, extension=None):
        self.data_sources.append((name, function))
        if extension is not None:
            self.extensions.append((name, extension))

    def update_gas_status(self, (id, data)):
        if id == 0x281:
            assert (len(data) >= 8)
            self.gas = data[1] * 256 + data[0]
            self.buttonGo = (data[-1] > 64)
            if self.filteredGas is None:
                self.filteredGas = self.gas
            else:
                self.filteredGas = SCALE_NEW * self.gas + (
                    1.0 - SCALE_NEW) * self.filteredGas
Пример #5
0
class JohnDeere(object):
    UPDATE_TIME_FREQUENCY = 5.0  #20.0  # Hz 

    def __init__(self, can=None):
        if can is None:
            self.can = CAN()
            self.can.resetModules()
        else:
            self.can = can
        self.canproxy = CANProxy(self.can)
        self.time = 0.0
        self.buttonGo = None  # TODO currently not available (!)
        self.desired_speed = 0.0
        self.drop_ball = False  # TODO move to ro.py only
        self.extensions = []
        self.data_sources = []
        self.modulesForRestart = []
        self.can.sendOperationMode()
  
    def __del__(self):
        self.can.sendPreoperationMode() 

    # TODO to move out, can.py??
    def check_modules(self, packet):
        # test if all modules are in Operation mode, if not restart them
        id, data = packet
        if id & 0xFF0 == 0x700: # heart beat message
            moduleId = id & 0xF
            assert( len(data) == 1 )
            if data[0] != 5:
                self.can.printPacket( id, data )
                if not moduleId in self.modulesForRestart:
                    self.modulesForRestart.append( moduleId )
                    print "RESET", moduleId
                    self.can.sendData( 0, [129,moduleId] )
#                if moduleId in [0x01, 0x02]:
#                    if (0x180 | moduleId) in self.encData:
#                      # The encoder information is invalid during a reset
#                      del self.encData[0x180 | moduleId]
            elif data[0] == 127: # restarted and in preoperation
                print "SWITCH TO OPERATION", moduleId
                self.can.sendData( 0, [1,moduleId] ) 
            elif moduleId in self.modulesForRestart:
                print "RUNNING", moduleId
                self.modulesForRestart.remove(moduleId)

    def register_data_source(self, name, function, extension=None):
        self.data_sources.append((name, function))
        if extension is not None:
            self.extensions.append((name, extension))

    def send_ball_dispenser(self):
        if self.drop_ball:
            cmd = 127
        else:
            cmd = 128
        self.can.sendData(0x37F, [0, cmd, 0, 0, 0, 0, 0, 0])

    def wait(self, duration):
        start_time = self.time
        while self.time - start_time < duration:
            self.update()

    def update(self):
        while True:
            packet = self.can.readPacket()
            self.canproxy.update(packet)
#            self.update_emergency_stop(packet)
            self.check_modules(packet)
            for (name,e) in self.extensions:
                e(self, packet[0], packet[1])
            
            # make sure that all updates get also termination SYNC (0x80)
            if packet[0] == 0x80:  
                break

        # send data related to other sources
        for (id,fce) in self.data_sources:
            data = fce()
            if data != None:
                for (name,e) in self.extensions:
                    e(self, id, data)

        self.time += 1.0/self.UPDATE_TIME_FREQUENCY  
        self.canproxy.set_time(self.time)
        self.canproxy.send_speed()
        self.send_ball_dispenser()

    def stop(self):
        "send stop command and make sure robot really stops"
        self.desired_speed = 0.0
        self.canproxy.stop()
        for i in xrange(10):
            self.update()
Пример #6
0
class Spider3(object):
    UPDATE_TIME_FREQUENCY = 20.0  # Hz

    # status word
    EMERGENCY_STOP = 0x0001
    DEVICE_READY = 0x0002  # also releaseState
    MASK_HEARTBEAT = 0x8000

    def __init__(self, can=None):
        if can is None:
            self.can = CAN()
#            self.can.resetModules()
        else:
            self.can = can
        self.time = 0.0
        self.status_word = None  # not defined yet
        self.wheel_angles = [None] * 4  # original CAN data
        self.drive_status = [None] * 4
        self.zero_steering = None  # zero position of all 4 wheels
        self.alive = 0  # togle with 128
        self.speed_cmd = [0, 0]
        self.status_cmd = 3
        self.extensions = []
        self.data_sources = []
        self.threads = []
        self.modulesForRestart = []
        self.led = 0
        self.led_time = 0.0
#        self.can.sendOperationMode()

    def __del__(self):
        pass
#        self.can.sendPreoperationMode()

# TODO to move out, can.py??

    def check_modules(self, packet):
        # test if all modules are in Operation mode, if not restart them
        id, data = packet
        if id & 0xFF0 == 0x700:  # heart beat message
            moduleId = id & 0xF
            assert (len(data) == 1)
            if data[0] != 5:
                self.can.printPacket(id, data)
                if not moduleId in self.modulesForRestart:
                    self.modulesForRestart.append(moduleId)
                    print("RESET", moduleId)
                    self.can.sendData(0, [129, moduleId])


#                if moduleId in [0x01, 0x02]:
#                    if (0x180 | moduleId) in self.encData:
#                      # The encoder information is invalid during a reset
#                      del self.encData[0x180 | moduleId]
            elif data[0] == 127:  # restarted and in preoperation
                print("SWITCH TO OPERATION", moduleId)
                self.can.sendData(0, [1, moduleId])
            elif moduleId in self.modulesForRestart:
                print("RUNNING", moduleId)
                self.modulesForRestart.remove(moduleId)

    def register_data_source(self, name, function, extension=None):
        self.data_sources.append((name, function))
        if extension is not None:
            self.extensions.append((name, extension))

    def wait(self, duration):
        start_time = self.time
        while self.time - start_time < duration:
            self.update()

    def update_status_word(self, packet):
        msg_id, data = packet
        if msg_id == 0x200:
            assert len(data) == 2, packet
            prev = self.status_word
            self.status_word = struct.unpack('H', bytearray(data))[0]
            if prev is not None:
                diff = prev ^ self.status_word
                is_alive = diff & self.MASK_HEARTBEAT
                if is_alive == 0:
                    # make sure nothing is missing (for now)
                    print("\n!!!Spider is DEAD!!! (or data are missing)\n")
                if (diff & 0x7FFF) != 0:
                    print("Status %d -> %d" %
                          (prev & 0x7FFF, self.status_word & 0x7FFF))

        elif msg_id == 0x201:
            assert len(data) == 8, packet
            prev = self.wheel_angles
            self.wheel_angles = struct.unpack('HHHH', bytearray(data))
            if prev != self.wheel_angles and self.zero_steering is not None:
                # note, that self.zero_steering has 200ms update rate,
                # i.e. it does not have to be immediately defined
                print('Wheels: %.2f' % self.time, [
                    fix_range(a - b)
                    for a, b in zip(self.wheel_angles, self.zero_steering)
                ])

        elif msg_id == 0x202:
            assert len(data) == 8, packet  # drive status + zero positions
            self.drive_status = struct.unpack('HHHH', bytearray(data))
            drive = self.drive_status[0] - self.drive_status[
                2], self.drive_status[3] - self.drive_status[1]
            if abs(drive[0]) + abs(drive[1]) > 10:
                print('Drive:', drive, abs(drive[0]) + abs(drive[1]))

        elif msg_id == 0x203:
            assert len(data) == 8, packet
            prev = self.zero_steering
            self.zero_steering = struct.unpack('HHHH', bytearray(data))
            # make sure that calibration did not change during program run
            assert prev is None or prev == self.zero_steering, (
                prev, self.zero_steering)

    def send_speed(self):
        self.can.sendData(0x400, [self.status_cmd, self.alive])
        self.can.sendData(0x401,
                          self.speed_cmd)  # drive control data drive, steering
        self.alive = 128 - self.alive

    def send_LED(self):
        self.can.sendData(0x410, [1, 2, 3, 4, 5, 6, 7, self.led])
        if self.time - self.led_time > 0.5:
            self.led = 1 - self.led
            self.led_time = self.time

    def set_raw_speed(self, fwd_rev_drive, left_right):
        print('set_raw_speed', fwd_rev_drive, left_right)
        self.speed_cmd = [fwd_rev_drive, left_right]

    def update(self):
        while True:
            packet = self.can.readPacket()
            self.check_modules(packet)
            self.update_status_word(packet)
            for (name, e) in self.extensions:
                e(self, packet[0], packet[1])

            # make sure that all updates get also termination SYNC (0x80)
            # there is no SYNC on Spider3 yet, use 0x200 for now
            if packet[0] == 0x200:
                break

        # send data related to other sources
        for (id, fce) in self.data_sources:
            data = fce()
            if data != None:
                for (name, e) in self.extensions:
                    e(self, id, data)

        self.time += 1.0 / self.UPDATE_TIME_FREQUENCY
        self.send_speed()
        self.send_LED()

    def stop(self):
        "send stop command and make sure robot really stops"
        self.set_raw_speed(0, 0)
        for i in range(10):
            self.update()
Пример #7
0
class JohnDeere(object):
    UPDATE_TIME_FREQUENCY = 5.0  #20.0  # Hz 

    def __init__(self, can=None, localization=None, config=None):
        if can is None:
            self.can = CAN()
            self.can.resetModules()
        else:
            self.can = can
        self.canproxy = CANProxy(self.can)
        self.localization = localization
        if config is not None:
            global TURN_ANGLE_OFFSET
            TURN_ANGLE_OFFSET = config.get('TURN_ANGLE_OFFSET', TURN_ANGLE_OFFSET)
        self.time = 0.0
        self.buttonGo = None  # TODO currently not available (!)
        self.drop_ball = False  # TODO move to ro.py only
        self.extensions = []
        self.data_sources = []
        self.threads = []
        self.modulesForRestart = []
        self.can.sendOperationMode()
  
    def __del__(self):
        self.can.sendPreoperationMode() 

    # TODO to move out, can.py??
    def check_modules(self, packet):
        # test if all modules are in Operation mode, if not restart them
        id, data = packet
        if id & 0xFF0 == 0x700: # heart beat message
            moduleId = id & 0xF
            assert( len(data) == 1 )
            if data[0] != 5:
                self.can.printPacket( id, data )
                if not moduleId in self.modulesForRestart:
                    self.modulesForRestart.append( moduleId )
                    print "RESET", moduleId
                    self.can.sendData( 0, [129,moduleId] )
#                if moduleId in [0x01, 0x02]:
#                    if (0x180 | moduleId) in self.encData:
#                      # The encoder information is invalid during a reset
#                      del self.encData[0x180 | moduleId]
            elif data[0] == 127: # restarted and in preoperation
                print "SWITCH TO OPERATION", moduleId
                self.can.sendData( 0, [1,moduleId] ) 
            elif moduleId in self.modulesForRestart:
                print "RUNNING", moduleId
                self.modulesForRestart.remove(moduleId)

    def register_data_source(self, name, function, extension=None):
        self.data_sources.append((name, function))
        if extension is not None:
            self.extensions.append((name, extension))

    def send_ball_dispenser(self):
        if self.drop_ball:
            cmd = 127
        else:
            cmd = 128
        self.can.sendData(0x37F, [0, cmd, 0, 0, 0, 0, 0, 0])

    def wait(self, duration):
        start_time = self.time
        while self.time - start_time < duration:
            self.update()

    def update(self):
        prev_enc = self.canproxy.dist_left_raw, self.canproxy.dist_right_raw
        while True:
            packet = self.can.readPacket()
            self.canproxy.update(packet)
#            self.update_emergency_stop(packet)
            self.check_modules(packet)
            for (name,e) in self.extensions:
                e(self, packet[0], packet[1])
            
            # make sure that all updates get also termination SYNC (0x80)
            if packet[0] == 0x80:  
                break
        
        if (self.localization is not None and
            prev_enc[0] is not None and prev_enc[1] is not None and
            self.canproxy.wheel_angle_raw is not None):
            dist_left = ENC_SCALE * (self.canproxy.dist_left_raw - prev_enc[0])
            dist_right = ENC_SCALE * (self.canproxy.dist_right_raw - prev_enc[1])
            angle_left = self.canproxy.wheel_angle_raw * TURN_SCALE + TURN_ANGLE_OFFSET
            self.localization.update_odometry(angle_left, dist_left, dist_right)

        # send data related to other sources
        for (id,fce) in self.data_sources:
            data = fce()
            if data != None:
                for (name,e) in self.extensions:
                    e(self, id, data)

        self.time += 1.0/self.UPDATE_TIME_FREQUENCY  
        self.canproxy.set_time(self.time)
        self.canproxy.send_speed()
        self.canproxy.send_LEDs()
        self.send_ball_dispenser()

    def set_desired_speed(self, speed):
        """set desired speed in meters per second.
        speed = None ... disable speed control
        ... can be called in every cycle without side-effects
        """
        self.canproxy.set_desired_speed_raw(int(speed/ENC_SCALE))

    def set_desired_steering(self, angle):
        """set desired steering angle of left wheel"""
        # angle = sensors['steering'] * TURN_SCALE + TURN_ANGLE_OFFSET  # radians
        raw = (angle - TURN_ANGLE_OFFSET)/TURN_SCALE
        self.canproxy.set_turn_raw(int(raw))

    def stop(self):
        "send stop command and make sure robot really stops"
        self.canproxy.stop()
        for i in xrange(10):
            self.update()
Пример #8
0
class JohnDeere(object):
    UPDATE_TIME_FREQUENCY = 5.0  #20.0  # Hz 

    def __init__(self, can=None, localization=None, config=None):
        if can is None:
            self.can = CAN()
            self.can.resetModules()
        else:
            self.can = can
        self.canproxy = CANProxy(self.can)
        self.localization = localization
        if config is not None:
            global TURN_ANGLE_OFFSET
            TURN_ANGLE_OFFSET = config.get('TURN_ANGLE_OFFSET', TURN_ANGLE_OFFSET)
        self.time = 0.0
        self.buttonGo = None  # TODO currently not available (!)
        self.drop_ball = False  # TODO move to ro.py only
        self.extensions = []
        self.data_sources = []
        self.threads = []
        self.modulesForRestart = []
        self.can.sendOperationMode()
  
    def __del__(self):
        self.can.sendPreoperationMode() 

    # TODO to move out, can.py??
    def check_modules(self, packet):
        # test if all modules are in Operation mode, if not restart them
        id, data = packet
        if id & 0xFF0 == 0x700: # heart beat message
            moduleId = id & 0xF
            assert( len(data) == 1 )
            if data[0] != 5:
                self.can.printPacket( id, data )
                if not moduleId in self.modulesForRestart:
                    self.modulesForRestart.append( moduleId )
                    print("RESET", moduleId)
                    self.can.sendData( 0, [129,moduleId] )
#                if moduleId in [0x01, 0x02]:
#                    if (0x180 | moduleId) in self.encData:
#                      # The encoder information is invalid during a reset
#                      del self.encData[0x180 | moduleId]
            elif data[0] == 127: # restarted and in preoperation
                print("SWITCH TO OPERATION", moduleId)
                self.can.sendData( 0, [1,moduleId] ) 
            elif moduleId in self.modulesForRestart:
                print("RUNNING", moduleId)
                self.modulesForRestart.remove(moduleId)

    def register_data_source(self, name, function, extension=None):
        self.data_sources.append((name, function))
        if extension is not None:
            self.extensions.append((name, extension))

    def send_ball_dispenser(self):
        if self.drop_ball:
            cmd = 127
        else:
            cmd = 128
        self.can.sendData(0x37F, [0, cmd, 0, 0, 0, 0, 0, 0])

    def wait(self, duration):
        start_time = self.time
        while self.time - start_time < duration:
            self.update()

    def update(self):
        prev_enc = self.canproxy.dist_left_raw, self.canproxy.dist_right_raw
        while True:
            packet = self.can.readPacket()
            self.canproxy.update(packet)
#            self.update_emergency_stop(packet)
            self.check_modules(packet)
            for (name,e) in self.extensions:
                e(self, packet[0], packet[1])
            
            # make sure that all updates get also termination SYNC (0x80)
            if packet[0] == 0x80:  
                break
        
        if (self.localization is not None and
            prev_enc[0] is not None and prev_enc[1] is not None and
            self.canproxy.wheel_angle_raw is not None):
            dist_left = ENC_SCALE * (self.canproxy.dist_left_raw - prev_enc[0])
            dist_right = ENC_SCALE * (self.canproxy.dist_right_raw - prev_enc[1])
            angle_left = self.canproxy.wheel_angle_raw * TURN_SCALE + TURN_ANGLE_OFFSET
            self.localization.update_odometry(angle_left, dist_left, dist_right)

        # send data related to other sources
        for (id,fce) in self.data_sources:
            data = fce()
            if data != None:
                for (name,e) in self.extensions:
                    e(self, id, data)

        self.time += 1.0/self.UPDATE_TIME_FREQUENCY  
        self.canproxy.set_time(self.time)
        self.canproxy.send_speed()
        self.canproxy.send_LEDs()
        self.send_ball_dispenser()

    def set_desired_speed(self, speed):
        """set desired speed in meters per second.
        speed = None ... disable speed control
        ... can be called in every cycle without side-effects
        """
        self.canproxy.set_desired_speed_raw(int(speed/ENC_SCALE))

    def set_desired_steering(self, angle):
        """set desired steering angle of left wheel"""
        # angle = sensors['steering'] * TURN_SCALE + TURN_ANGLE_OFFSET  # radians
        raw = (angle - TURN_ANGLE_OFFSET)/TURN_SCALE
        self.canproxy.set_turn_raw(int(raw))

    def stop(self):
        "send stop command and make sure robot really stops"
        self.canproxy.stop()
        for i in range(10):
            self.update()