예제 #1
0
class Create2:
    def __init__(self, tty="/dev/ttyUSB0", threading=False, interval=500):
        time.sleep(1)
        self.sci = SerialCommandInterface(tty, baudrate=BAUDRATE, timeout=SERIAL_TIMEOUT)
        self.opcode = Opcode(self.sci)
        if threading:
            self.observer = SensorObserver(self.sci, interval)
            self.observer.start()
        self.opcode.start()
        self.opcode.safe()
    
    def start(self):
        self.opcode.start
    def stop(self):
        self.opcode.stop

    def set_mode(self, modes):
        if(modes==Modes.Safe):
            self.opcode.safe
        elif(modes==Modes.Full):
            self.opcode.full
        elif(modes==Modes.Passive):
            self.opcode.start
        elif(modes==Modes.OFF):
            self.power
        
    def drive(self, velocity, radius):
        velocity = int(velocity) & 0xffff
        radius = int(radius) & 0xffff
        data = struct.unpack('4B', struct.pack('>2H', velocity, radius))
        self.opcode.drive(data)
    
    # left and right : wheel velocity(-500 - 500ms/s)
    def drive_wheels(self, left, right):
        args = struct.unpack('4B', struct.pack('>2H', right, left))
        self.opcode.driveWheels(args)
    
    # left and right : motor PWM (-255 - 255)
    def drive_pwm(self, left, right):
        args = struct.unpack('4B', struct.pack('>2H', right, left))
        self.opcode.drivePwm(args)
        
    def brush(self, mainBrush, vacuum, sideBrush):
        self.opcode.motors( (mainBrush << 2 | vacuum<<1 | sideBrush) )
        
    # mainBrushPWM and sideBrushPWM : PWM (-127 - 127)
    # vacuumPWM : PWM (0 - 127)
    def brush_pwm(self, mainBrushPWM, vacuumPWM, sideBrushPWM):
        self.opcode.pwmMotors( [mainBrushPWM, vacuumPWM, sideBrushPWM] )
    
    def docking(self):
        self.opcode.forceSeekingDock
    
    # args : ascii code
    def digit_leds_ascii(self, digit3ascii, digit2ascii, digit1ascii, digit0ascii):
        self.opcode.digitLedsAscii([digit3ascii, digit2ascii, digit1ascii, digit0ascii])

    def request_sensor(self, packetID, numBytes):
        self.sci.flash_input()
        requestBytes = [142, packetID]
        self.sci.send(requestBytes);
        data = self.sci.read(numBytes)
        return data
        
    def request_all_sensor(self):
        self.sci.flash_input()
        requestBytes = [142, 100]
        self.sci.send(requestBytes)
        data = self.sci.read(80)
        return Sensor.gen_from_bytes(data)

# for multithread
    def get_distance(self):
        return self.observer.get_distance()
    def get_angle(self):
        return self.observer.get_angle()
    
    def get_left_encoder(self):
        return self.observer.get_left_encoder()
    
    def get_right_encoder(self):
        return self.observer.get_right_encoder()
    
    def get_sensor(self):
        return self.observer.get_sensor()
    
    def get_sensor_raw(self):
        return self.observer.get_raw_sensor()

    def add_event_listener(self, listener):
        self.observer.add_listener(listener)
    def set_next_distance(self, distance, greater):
        self.observer.set_next_distance(distance, greater)
    def set_next_angle(self, angle, greater):
        self.observer.set_next_angle(angle, greater)
예제 #2
0
class Create2:
    def __init__(self, tty="/dev/ttyUSB0", threading=False, interval=500):
        time.sleep(2)
        self.sci = SerialCommandInterface(tty,
                                          baudrate=BAUDRATE,
                                          timeout=SERIAL_TIMEOUT)
        self.opcode = Opcode(self.sci)
        if threading:
            self.observer = SensorObserver(self.sci, interval)
            self.observer.start()
        self.opcode.start()
        self.opcode.safe()
        time.sleep(1)

    def start(self):
        self.opcode.start

    def stop(self):
        self.opcode.stop

    def set_mode(self, modes):
        if (modes == Modes.Safe):
            self.opcode.safe
        elif (modes == Modes.Full):
            self.opcode.full
        elif (modes == Modes.Passive):
            self.opcode.start
        elif (modes == Modes.OFF):
            self.power

    def drive(self, velocity, radius):
        velocity = int(velocity) & 0xffff
        radius = int(radius) & 0xffff
        data = struct.unpack('4B', struct.pack('>2H', velocity, radius))
        self.opcode.drive(data)

    # left and right : wheel velocity(-500 - 500ms/s)
    def drive_wheels(self, left, right):
        args = struct.unpack('4B', struct.pack('>2H', right, left))
        self.opcode.driveWheels(args)

    # left and right : motor PWM (-255 - 255)
    def drive_pwm(self, left, right):
        args = struct.unpack('4B', struct.pack('>2H', right, left))
        self.opcode.drivePwm(args)

    def brush(self, mainBrush, vacuum, sideBrush):
        self.opcode.motors((mainBrush << 2 | vacuum << 1 | sideBrush))

    # mainBrushPWM and sideBrushPWM : PWM (-127 - 127)
    # vacuumPWM : PWM (0 - 127)
    def brush_pwm(self, mainBrushPWM, vacuumPWM, sideBrushPWM):
        self.opcode.pwmMotors([mainBrushPWM, vacuumPWM, sideBrushPWM])

    def docking(self):
        self.opcode.forceSeekingDock

    # args : ascii code
    def digit_leds_ascii(self, digit3ascii, digit2ascii, digit1ascii,
                         digit0ascii):
        self.opcode.digitLedsAscii(
            [digit3ascii, digit2ascii, digit1ascii, digit0ascii])

    def request_sensor(self, packetID, numBytes):
        self.sci.flash_input()
        requestBytes = [142, packetID]
        self.sci.send(requestBytes)
        data = self.sci.read(numBytes)
        return data

    def request_all_sensor(self):
        self.sci.flash_input()
        requestBytes = [142, 100]
        self.sci.send(requestBytes)
        data = self.sci.read(80)
        return Sensor.gen_from_bytes(data)

# for multithread

    def get_distance(self):
        return self.observer.get_distance()

    def get_angle(self):
        return self.observer.get_angle()

    def get_left_encoder(self):
        return self.observer.get_left_encoder()

    def get_right_encoder(self):
        return self.observer.get_right_encoder()

    def get_sensor(self):
        return self.observer.get_sensor()

    def get_sensor_raw(self):
        return self.observer.get_raw_sensor()

    def add_event_listener(self, listener):
        self.observer.add_listener(listener)

    def set_next_distance(self, distance, greater):
        self.observer.set_next_distance(distance, greater)

    def set_next_angle(self, angle, greater):
        self.observer.set_next_angle(angle, greater)
class Create2(object):
    __instance = None

    def __new__(cls, *args, **keys):
        if cls.__instance is None:
            cls.__instance = object.__new__(cls, *args, **keys)
            cls.__instance.__initialized = False
        return cls.__instance

    def __init__(self, tty="/dev/ttyUSB0", threading=False, interval=50):
        if (self.__instance.__initialized): return
        self.__instance.__initialized = True
        time.sleep(2)

        self.correction_value = 1.0
        self.ci = 0.0
        self.k = 0.15

        self.sci = SerialCommandInterface(tty,
                                          baudrate=BAUDRATE,
                                          timeout=SERIAL_TIMEOUT)
        self.opcode = Opcode(self.sci)
        if threading:
            self.observer = SensorObserver(self.sci, interval)
            self.observer.start()
        self.opcode.start()
        self.opcode.safe()
        time.sleep(1)

    def start(self):
        self.opcode.start()

    def stop(self):
        self.opcode.stop()

    def set_mode(self, modes):
        if (modes == Modes.Safe):
            self.opcode.safe()
        elif (modes == Modes.Full):
            self.opcode.full()
        elif (modes == Modes.Passive):
            self.opcode.start()
        elif (modes == Modes.OFF):
            self.opcode.power()

    def drive(self, velocity, radius):
        velocity = int(velocity) & 0xffff
        radius = int(radius) & 0xffff
        data = struct.unpack('4B', struct.pack('>2H', velocity, radius))
        self.opcode.drive(data)

    # left and right : wheel velocity(-500 - 500ms/s)
    def drive_wheels(self, left, right):
        args = struct.unpack('4B', struct.pack('>2H', right, left))
        self.opcode.driveWheels(args)

    # drive pid
    def drive_pid(self, target, power):
        angle = self.get_angle()

        e = target - angle

        if abs(e) < 2.0:
            kp = 0.6
            ki = 25.0
        else:
            kp = 5.0
            ki = 4.0
            # self.ci = self.ci/2

        # P項目計算
        cp = e * kp

        # I項目計算
        if ((cp >= 0) and (self.ci < 0)) or ((cp < 0) and (self.ci >= 0)):
            self.ci = self.ci + e * ki * 10 * 0.05
        else:
            self.ci = self.ci + e * ki * 0.05

        # TODO:I項リミッタ

        if power == 0:
            c = int(cp + self.ci)
        else:
            c = int((cp + self.ci) * 150 / power)
        self.drive_pwm(power - c, power + c)

    # left and right : motor PWM (-255 - 255)
    def drive_pwm(self, left, right):
        lc = left * self.correction_value
        rc = right * self.correction_value

        if lc > 250:
            lc = 250
        elif lc < -250:
            lc = -250

        if rc > 250:
            rc = 250
        elif rc < -250:
            rc = 250

        args = struct.unpack('4B', struct.pack('>2H', rc, lc))
        self.opcode.drivePwm(args)

    def brush(self, mainBrush, vacuum, sideBrush):
        self.opcode.motors((mainBrush << 2 | vacuum << 1 | sideBrush))

    # mainBrushPWM and sideBrushPWM : PWM (-127 - 127)
    # vacuumPWM : PWM (0 - 127)
    def brush_pwm(self, mainBrushPWM, vacuumPWM, sideBrushPWM):
        self.opcode.pwmMotors([mainBrushPWM, vacuumPWM, sideBrushPWM])

    def docking(self):
        self.opcode.forceSeekingDock

    # args : ascii code
    def digit_leds_ascii(self, digit3ascii, digit2ascii, digit1ascii,
                         digit0ascii):
        self.opcode.digitLedsAscii(
            [digit3ascii, digit2ascii, digit1ascii, digit0ascii])

    def request_sensor(self, packetID, numBytes):
        self.sci.flash_input()
        requestBytes = [142, packetID]
        self.sci.send(requestBytes)
        data = self.sci.read(numBytes)
        return data

    def request_all_sensor(self):
        self.sci.flash_input()
        requestBytes = [142, 100]
        self.sci.send(requestBytes)
        data = self.sci.read(80)
        return Sensor.gen_from_bytes(data)

# for multithread

    def get_distance(self):
        return self.observer.get_distance()

    def get_angle(self):
        return self.observer.get_angle()

    def get_left_encoder(self):
        return self.observer.get_left_encoder()

    def get_right_encoder(self):
        return self.observer.get_right_encoder()

    def get_sensor(self):
        return self.observer.get_sensor()

    def get_sensor_raw(self):
        return self.observer.get_raw_sensor()

    def add_event_listener(self, listener):
        self.observer.add_listener(listener)

    def set_next_distance(self, distance, greater):
        self.observer.set_next_distance(distance, greater)

    def set_next_angle(self, angle, greater):
        self.observer.set_next_angle(angle, greater)

    def set_correction_value(selff, val):
        self.correction_value = val