コード例 #1
0
ファイル: johndeere.py プロジェクト: myrobot2017/osgar
 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()
コード例 #2
0
 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()
コード例 #3
0
ファイル: johndeere.py プロジェクト: robotika/osgar
 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()
コード例 #4
0
ファイル: johndeere.py プロジェクト: luciusb/osgar
 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()
コード例 #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
ファイル: johndeere.py プロジェクト: myrobot2017/osgar
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()
コード例 #7
0
ファイル: johndeere.py プロジェクト: robotika/osgar
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()