Exemple #1
0
    def __init__(self, bus_number=None, address=0x40, frequency = 60, drive = 0, steer = 1, drive_n = 369, steer_c = 390):
        ''' This is very important part of set ESC.
        If you want to drive motor by this source, you have to use pca9685 drive.
        Because the ESC circuit is connected with PCA9685 PWM circuit board's channel pins

        Argument
        bus_number : bus type of raspberry Pi. If it doesn't set, pca9685 module set value as default.
        address : I2C slave address
        frequency : driving motor(forward/backward motor) PWM frequency.
        driver : pca9685 channel number of driving motor
        steer : pca9685 channel number of steering motor
        '''
        self.pwm = PWM(bus_number, address)
        self.pwm.frequency = frequency
        self.pwm.setup()
        self.drive = drive
        self.steer = steer
        #self.steer_NEUTRAL = 390 #default value. It needs to calibrate
        self.steer_NEUTRAL = steer_c
        self.steer_MIN = 280
        self.steer_MAX = 500
        #self.drive_NEUTRAL = 369 #default value. It needs to calibrate
        self.drive_NEUTRAL = drive_n
        self.drive_MIN = 280
        self.drive_MAX = 450
        self.steer_val = self.steer_NEUTRAL
        self.drive_val = self.drive_NEUTRAL
        self.speed_forward = self.steer_NEUTRAL
        self.speed_backward = self.steer_NEUTRAL
        self.steer_diff = 35
        self.drive_diff = 5
        self.is_stop = True
Exemple #2
0
class ESC(object):
    '''
    This class is made to use ESC device in Raspberry Pi.
    Becareful when you connect ESC to your Raspberry Pi board.
    It needs very large current, it can make broken your circuit.

    '''
    def __init__(self, bus_number=None, address=0x40, frequency = 60, drive = 0, steer = 1, drive_n = 369, steer_c = 390):
        ''' This is very important part of set ESC.
        If you want to drive motor by this source, you have to use pca9685 drive.
        Because the ESC circuit is connected with PCA9685 PWM circuit board's channel pins

        Argument
        bus_number : bus type of raspberry Pi. If it doesn't set, pca9685 module set value as default.
        address : I2C slave address
        frequency : driving motor(forward/backward motor) PWM frequency.
        driver : pca9685 channel number of driving motor
        steer : pca9685 channel number of steering motor
        '''
        self.pwm = PWM(bus_number, address)
        self.pwm.frequency = frequency
        self.pwm.setup()
        self.drive = drive
        self.steer = steer
        #self.steer_NEUTRAL = 390 #default value. It needs to calibrate
        self.steer_NEUTRAL = steer_c
        self.steer_MIN = 280
        self.steer_MAX = 500
        #self.drive_NEUTRAL = 369 #default value. It needs to calibrate
        self.drive_NEUTRAL = drive_n
        self.drive_MIN = 280
        self.drive_MAX = 450
        self.steer_val = self.steer_NEUTRAL
        self.drive_val = self.drive_NEUTRAL
        self.speed_forward = self.steer_NEUTRAL
        self.speed_backward = self.steer_NEUTRAL
        self.steer_diff = 35
        self.drive_diff = 5
        self.is_stop = True

    def calibrate_drive_NEUTRAL(self, cal_value = 390):
        if cal_value > self.drive_MAX or cal_value < self.drive_MIN:
            print "Calibration value Fail"
        else:
            print "Drive Calib set to %d" % cal_value
            self.drive_NEUTRAL = cal_value

    def calibrate_steer_NEUTRAL(self, cal_value = 390):
        if cal_value > self.steer_MAX or cal_value < self.steer_MIN:
            print "Stree Calibration value Fail"
        else:
            print "Steer Calib set to %d" % cal_value
            self.steer_NEUTRAL = cal_value

    def set_steer_strength(self, strength = 35):
        if strength > (self.steer_MAX - self.steer_NEUTRAL):
            strength = self.steer_MAX - self.steer_NEUTRAL
        elif strength < (self.steer_NEUTRAL - self.steer_NEUTRAL):
            strength = self.steer_NEUTRAL - self.steer_NEUTRAL
        self.steer_diff = strength
    def __init__(self):
        self.state = 2
        self.fPWM = 50
        self.i2c_address = 0x40 # (standard) adapt to your module
        self.channel = 0 # adapt to your wiring
        self.a = 8.5 # adapt to your servo
        self.b = 2  # adapt to your servo

        self.bus = SMBus(3) # Raspberry Pi revision 2
        self.pwm = PWM(self.bus, self.i2c_address)
        self.pwm.setFreq(self.fPWM)

        self.setup()

        self.zeroBot()

        self.frLeg = Leg3d(side=1)
        self.flLeg = Leg3d(side=2)
        self.lrLeg = Leg3d(side=2)
        self.rrLeg = Leg3d(side=1)
        self.t = 0

        # walker = Walk3d(stride_height=-0.01,stride_length =.02)

        bpm = 138#music tempo
        self.freq = bpm/60.*2*pi
        self.amp = 0.015
Exemple #4
0
def setup():
    global pwm
    bus = SMBus(1)  # Raspberry Pi revision 2
    pwm[0] = PWM(bus, i2c_address)
    pwm[0].setFreq(fPWM)
    pwm[1] = PWM(bus, i2c_address + 1)
    pwm[1].setFreq(fPWM)
    def __init__(self, drive, steer, bus_number=None, address=0x40, frequency = 60, drive_ch = 0, steer_ch = 1):
        ''' This is very important part of set ESC.
        If you want to drive motor by this source, you have to use pca9685 drive.
        Because the ESC circuit is connected with PCA9685 PWM circuit board's channel pins

        Argument
        bus_number : bus type of raspberry Pi. If it doesn't set, pca9685 module set value as default.
        address : I2C slave address
        frequency : driving motor(forward/backward motor) PWM frequency.
        driver : pca9685 channel number of driving motor
        steer : pca9685 channel number of steering motor
        '''

        #mode - 0 : forward, 1 : backward, 2 : Neutral, 3 : None
        self.set_mode = 3
        self.steer_value_loaded = False
        self.drive_value_loaded = False
        self.current_mode = ["Forward", "Backward", "Neutral", "None"]

        #Set Channel
        self.drive_ch = drive_ch 
        self.steer_ch = steer_ch

        #Default values for steer
        #self.steer_NEUTRAL = 390 
        self.steer_center = steer
        self.steer_MIN = 280
        self.steer_MAX = 500
        self.steer_val = 0
        self.steer_diff = 15

        #Default values for drive(engine)
        #self.drive_NEUTRAL = 369 
        self.drive_neutral = drive 
        self.drive_MIN = 280
        self.drive_MAX = 450
        #self.drive_val = self.drive_neutral
        self.speed_forward = 0
        self.speed_backward = 0
        self.drive_diff = 5

        # PWM init
        self.pwm = PWM(bus_number, address)
        self.pwm.frequency = frequency
        self.pwm.setup()
        time.sleep(0.1)
        self.pwm.write(self.drive_ch,0,0)
        self.pwm.write(self.steer_ch,0,0)
Exemple #6
0
 def __init__(self):
     self.pwm = PWM(0x40)
     self.pwm.set_pwm_freq(60)
     self.backwardMax = 350
     self.forwardMax = 300
     self.neutralbackandforth = 333
     self.rightMax = 397
     self.leftMax = 466
     self.neutralleftandright = 436
     #used to control eye lights via servo controllers. Workaround for lack of physical resources. Instead, probably implement like the error light in controller.py
     self.leftrightservo = 0
     self.forwardbackwardservo = 1
     self.leftEyeLed = 13
     self.rightEyeLed = 14
Exemple #7
0
    def __init__(self, ipAddress="", buttonEvent=None):
        """
        Creates an instance of MyRobot and initalizes the GPIO.
        """
        if MyRobot._myInstance != None:
            raise Exception("Only one instance of MyRobot allowed")
        global _isButtonEnabled, _buttonListener

        _buttonListener = buttonEvent

        # Use physical pin numbers
        GPIO.setmode(GPIO.BOARD)
        GPIO.setwarnings(False)

        # Left motor
        GPIO.setup(SharedConstants.P_LEFT_FORWARD, GPIO.OUT)
        SharedConstants.LEFT_MOTOR_PWM[0] = GPIO.PWM(SharedConstants.P_LEFT_FORWARD, SharedConstants.MOTOR_PWM_FREQ)
        SharedConstants.LEFT_MOTOR_PWM[0].start(0)
        GPIO.setup(SharedConstants.P_LEFT_BACKWARD, GPIO.OUT)
        SharedConstants.LEFT_MOTOR_PWM[1] = GPIO.PWM(SharedConstants.P_LEFT_BACKWARD, SharedConstants.MOTOR_PWM_FREQ)
        SharedConstants.LEFT_MOTOR_PWM[1].start(0)

        # Right motor
        GPIO.setup(SharedConstants.P_RIGHT_FORWARD, GPIO.OUT)
        SharedConstants.RIGHT_MOTOR_PWM[0] = GPIO.PWM(SharedConstants.P_RIGHT_FORWARD, SharedConstants.MOTOR_PWM_FREQ)
        SharedConstants.RIGHT_MOTOR_PWM[0].start(0)
        GPIO.setup(SharedConstants.P_RIGHT_BACKWARD, GPIO.OUT)
        SharedConstants.RIGHT_MOTOR_PWM[1] = GPIO.PWM(SharedConstants.P_RIGHT_BACKWARD, SharedConstants.MOTOR_PWM_FREQ)
        SharedConstants.RIGHT_MOTOR_PWM[1].start(0)

        # IR sensors
        GPIO.setup(SharedConstants.P_FRONT_LEFT, GPIO.IN, GPIO.PUD_UP)
        GPIO.setup(SharedConstants.P_FRONT_CENTER, GPIO.IN, GPIO.PUD_UP)
        GPIO.setup(SharedConstants.P_FRONT_RIGHT, GPIO.IN, GPIO.PUD_UP)
        GPIO.setup(SharedConstants.P_LINE_LEFT, GPIO.IN, GPIO.PUD_UP)
        GPIO.setup(SharedConstants.P_LINE_RIGHT, GPIO.IN, GPIO.PUD_UP)

        # Establish event recognition from battery monitor
        GPIO.setup(SharedConstants.P_BATTERY_MONITOR, GPIO.IN, GPIO.PUD_UP)
        GPIO.add_event_detect(SharedConstants.P_BATTERY_MONITOR, GPIO.RISING, _onBatteryDown)

        Tools.debug("Trying to detect I2C bus")
        isSMBusAvailable = True
        self._bus = None
        try:
            if GPIO.RPI_REVISION > 1:
                self._bus = smbus.SMBus(1)  # For revision 2 Raspberry Pi
                Tools.debug("Found SMBus for revision 2")
            else:
                self._bus = smbus.SMBus(0)  # For revision 1 Raspberry Pi
                Tools.debug("Found SMBus for revision 1")
        except:
            print "No SMBus found on this robot device."
            isSMBusAvailable = False

        # I2C PWM chip PCM9685 for LEDs and servos
        if isSMBusAvailable:
            self.pwm = PWM(self._bus, SharedConstants.PWM_I2C_ADDRESS)
            self.pwm.setFreq(SharedConstants.PWM_FREQ)
            # clear all LEDs
            for id in range(3):
                self.pwm.setDuty(3 * id, 0)
                self.pwm.setDuty(3 * id + 1, 0)
                self.pwm.setDuty(3 * id + 2, 0)

        # I2C analog extender chip
        if isSMBusAvailable:
            Tools.debug("Trying to detect PCF8591P I2C expander")
            channel = 0
            try:
                self._bus.write_byte(SharedConstants.ADC_I2C_ADDRESS, channel)
                self._bus.read_byte(SharedConstants.ADC_I2C_ADDRESS)  # ignore reply
                data = self._bus.read_byte(SharedConstants.ADC_I2C_ADDRESS)
                Tools.debug("Found PCF8591P I2C expander")
            except:
                Tools.debug("PCF8591P I2C expander not found")

        Tools.debug("Trying to detect 7-segment display")
        if isSMBusAvailable:
            self.displayType = "none"
            try:
                addr = 0x20
                rc = self._bus.read_byte_data(addr, 0)
                if rc != 0xA0:  # returns 255 for 4tronix
                    raise Exception()
                Tools.delay(100)
                self.displayType = "didel1"
            except:
                Tools.debug("'didel1' display not found")
            if self.displayType == "none":
                try:
                    addr = 0x20
                    self._bus.write_byte_data(addr, 0x00, 0x00)  # Set all of bank 0 to outputs
                    Tools.delay(100)
                    self.displayType = "4tronix"
                except:
                    Tools.debug("'4tronix' display not found")
                if self.displayType == "none":
                    try:
                        addr = 0x24
                        data = [0] * 4
                        self._bus.write_i2c_block_data(addr, data[0], data[1:])  # trying to clear display
                        self.displayType = "didel"
                    except:
                        Tools.debug("'didel' display not found")

            Tools.debug("Display type '" + self.displayType + "'")

            # Initializing (clear) display, if available
            if self.displayType == "4tronix":
                Disp4tronix().clear()
            if self.displayType == "didel":
                DgTell().clear()
            if self.displayType == "didel1":
                DgTell1().clear()

        GPIO.setup(SharedConstants.P_BUTTON, GPIO.IN, GPIO.PUD_UP)
        # Establish event recognition from button event
        GPIO.add_event_detect(SharedConstants.P_BUTTON, GPIO.BOTH, _onButtonEvent)
        _isButtonEnabled = True
        Tools.debug("MyRobot instance created. Lib Version: " + SharedConstants.VERSION)
        self.sensorThread = None
        MyRobot._myInstance = self
Exemple #8
0
class MyRobot(object):
    """
    Singleton class that represents a robot.
    Signature for the butten event callback: buttonEvent(int).
    (BUTTON_PRESSED, BUTTON_RELEASED, BUTTON_LONGPRESSED defined in ShareConstants.)
    @param ipAddress the IP address (default: None for autonomous mode)
    @param buttonEvent the callback function for pushbutton events (default: None)
    """

    _myInstance = None

    def __init__(self, ipAddress="", buttonEvent=None):
        """
        Creates an instance of MyRobot and initalizes the GPIO.
        """
        if MyRobot._myInstance != None:
            raise Exception("Only one instance of MyRobot allowed")
        global _isButtonEnabled, _buttonListener

        _buttonListener = buttonEvent

        # Use physical pin numbers
        GPIO.setmode(GPIO.BOARD)
        GPIO.setwarnings(False)

        # Left motor
        GPIO.setup(SharedConstants.P_LEFT_FORWARD, GPIO.OUT)
        SharedConstants.LEFT_MOTOR_PWM[0] = GPIO.PWM(SharedConstants.P_LEFT_FORWARD, SharedConstants.MOTOR_PWM_FREQ)
        SharedConstants.LEFT_MOTOR_PWM[0].start(0)
        GPIO.setup(SharedConstants.P_LEFT_BACKWARD, GPIO.OUT)
        SharedConstants.LEFT_MOTOR_PWM[1] = GPIO.PWM(SharedConstants.P_LEFT_BACKWARD, SharedConstants.MOTOR_PWM_FREQ)
        SharedConstants.LEFT_MOTOR_PWM[1].start(0)

        # Right motor
        GPIO.setup(SharedConstants.P_RIGHT_FORWARD, GPIO.OUT)
        SharedConstants.RIGHT_MOTOR_PWM[0] = GPIO.PWM(SharedConstants.P_RIGHT_FORWARD, SharedConstants.MOTOR_PWM_FREQ)
        SharedConstants.RIGHT_MOTOR_PWM[0].start(0)
        GPIO.setup(SharedConstants.P_RIGHT_BACKWARD, GPIO.OUT)
        SharedConstants.RIGHT_MOTOR_PWM[1] = GPIO.PWM(SharedConstants.P_RIGHT_BACKWARD, SharedConstants.MOTOR_PWM_FREQ)
        SharedConstants.RIGHT_MOTOR_PWM[1].start(0)

        # IR sensors
        GPIO.setup(SharedConstants.P_FRONT_LEFT, GPIO.IN, GPIO.PUD_UP)
        GPIO.setup(SharedConstants.P_FRONT_CENTER, GPIO.IN, GPIO.PUD_UP)
        GPIO.setup(SharedConstants.P_FRONT_RIGHT, GPIO.IN, GPIO.PUD_UP)
        GPIO.setup(SharedConstants.P_LINE_LEFT, GPIO.IN, GPIO.PUD_UP)
        GPIO.setup(SharedConstants.P_LINE_RIGHT, GPIO.IN, GPIO.PUD_UP)

        # Establish event recognition from battery monitor
        GPIO.setup(SharedConstants.P_BATTERY_MONITOR, GPIO.IN, GPIO.PUD_UP)
        GPIO.add_event_detect(SharedConstants.P_BATTERY_MONITOR, GPIO.RISING, _onBatteryDown)

        Tools.debug("Trying to detect I2C bus")
        isSMBusAvailable = True
        self._bus = None
        try:
            if GPIO.RPI_REVISION > 1:
                self._bus = smbus.SMBus(1)  # For revision 2 Raspberry Pi
                Tools.debug("Found SMBus for revision 2")
            else:
                self._bus = smbus.SMBus(0)  # For revision 1 Raspberry Pi
                Tools.debug("Found SMBus for revision 1")
        except:
            print "No SMBus found on this robot device."
            isSMBusAvailable = False

        # I2C PWM chip PCM9685 for LEDs and servos
        if isSMBusAvailable:
            self.pwm = PWM(self._bus, SharedConstants.PWM_I2C_ADDRESS)
            self.pwm.setFreq(SharedConstants.PWM_FREQ)
            # clear all LEDs
            for id in range(3):
                self.pwm.setDuty(3 * id, 0)
                self.pwm.setDuty(3 * id + 1, 0)
                self.pwm.setDuty(3 * id + 2, 0)

        # I2C analog extender chip
        if isSMBusAvailable:
            Tools.debug("Trying to detect PCF8591P I2C expander")
            channel = 0
            try:
                self._bus.write_byte(SharedConstants.ADC_I2C_ADDRESS, channel)
                self._bus.read_byte(SharedConstants.ADC_I2C_ADDRESS)  # ignore reply
                data = self._bus.read_byte(SharedConstants.ADC_I2C_ADDRESS)
                Tools.debug("Found PCF8591P I2C expander")
            except:
                Tools.debug("PCF8591P I2C expander not found")

        Tools.debug("Trying to detect 7-segment display")
        if isSMBusAvailable:
            self.displayType = "none"
            try:
                addr = 0x20
                rc = self._bus.read_byte_data(addr, 0)
                if rc != 0xA0:  # returns 255 for 4tronix
                    raise Exception()
                Tools.delay(100)
                self.displayType = "didel1"
            except:
                Tools.debug("'didel1' display not found")
            if self.displayType == "none":
                try:
                    addr = 0x20
                    self._bus.write_byte_data(addr, 0x00, 0x00)  # Set all of bank 0 to outputs
                    Tools.delay(100)
                    self.displayType = "4tronix"
                except:
                    Tools.debug("'4tronix' display not found")
                if self.displayType == "none":
                    try:
                        addr = 0x24
                        data = [0] * 4
                        self._bus.write_i2c_block_data(addr, data[0], data[1:])  # trying to clear display
                        self.displayType = "didel"
                    except:
                        Tools.debug("'didel' display not found")

            Tools.debug("Display type '" + self.displayType + "'")

            # Initializing (clear) display, if available
            if self.displayType == "4tronix":
                Disp4tronix().clear()
            if self.displayType == "didel":
                DgTell().clear()
            if self.displayType == "didel1":
                DgTell1().clear()

        GPIO.setup(SharedConstants.P_BUTTON, GPIO.IN, GPIO.PUD_UP)
        # Establish event recognition from button event
        GPIO.add_event_detect(SharedConstants.P_BUTTON, GPIO.BOTH, _onButtonEvent)
        _isButtonEnabled = True
        Tools.debug("MyRobot instance created. Lib Version: " + SharedConstants.VERSION)
        self.sensorThread = None
        MyRobot._myInstance = self

    def registerSensor(self, sensor):
        if self.sensorThread == None:
            self.sensorThread = SensorThread()
            self.sensorThread.start()
        self.sensorThread.add(sensor)

    def exit(self):
        """
        Cleans-up and releases all resources.
        """
        global _isButtonEnabled
        Tools.debug("Calling Robot.exit()")
        self.setButtonEnabled(False)

        # Stop motors
        SharedConstants.LEFT_MOTOR_PWM[0].ChangeDutyCycle(0)
        SharedConstants.LEFT_MOTOR_PWM[1].ChangeDutyCycle(0)
        SharedConstants.RIGHT_MOTOR_PWM[0].ChangeDutyCycle(0)
        SharedConstants.RIGHT_MOTOR_PWM[1].ChangeDutyCycle(0)

        # Stop button thread, if necessary
        if _buttonThread != None:
            _buttonThread.stop()

        # Stop display
        display = Display._myInstance
        if display != None:
            display.stopTicker()
            display.clear()

        Led.clearAll()
        MyRobot.closeSound()

        if self.sensorThread != None:
            self.sensorThread.stop()
            self.sensorThread.join(2000)

        # GPIO.cleanup() Do not cleanup, otherwise button will not work any more when coming back
        # from remote execution
        Tools.delay(2000)  # avoid "sys.excepthook is missing"

    def isButtonDown(self):
        """
        Checks if button is currently pressed.
        @return: True, if the button is actually pressed
        """
        Tools.delay(1)
        return GPIO.input(SharedConstants.P_BUTTON) == GPIO.LOW

    def isButtonHit(self):
        """
        Checks, if the button was ever hit or hit since the last invocation.
        @return: True, if the button was hit; otherwise False
        """
        global _isBtnHit
        Tools.delay(1)
        hit = _isBtnHit
        _isBtnHit = False
        return hit

    def isEscapeHit(self):
        """
        Same as isButtonHit() for compatibility with remote mode.
        """
        return self.isButtonHit()

    def isEnterHit(self):
        """
        Empty method for compatibility with remote mode.
        """
        pass

    def isUpHit(self):
        """
        Empty method for compatibility with remote mode.
        """
        pass

    def isDownHit(self):
        """
        Empty method for compatibility with remote mode.
        """
        pass

    def isLeftHit(self):
        """
        Empty method for compatibility with remote mode.
        """
        pass

    def isRightHit(self):
        """
        Empty method for compatibility with remote mode.
        """
        pass

    def addButtonListener(self, listener, enableClick=False, doubleClickTime=SharedConstants.BUTTON_DOUBLECLICK_TIME):
        """
        Registers a listener function to get notifications when the pushbutton is pressed, released or long pressed.
        If enableClick = True, in addition click and double-click events are reported. The click event not immediately
        reported, but only if within the doubleClickTime no other click is gererated.
        The value are defined as ShareConstants.BUTTON_PRESSED, ShareConstants.BUTTON_LONGPRESSED, ShareConstants.BUTTON_RELEASED,
        ShareConstants.BUTTON_CLICKED, ShareConstants.BUTTON_DOUBLECLICKED.
        With enableClick = False and the button is long pressed and released the sequence is: BUTTON_PRESSED, BUTTON_LONGPRESSED, BUTTON_RELEASED.
        With enableClick = True the sequences are the following:
        click: BUTTON_PRESSED, BUTTON_RELEASED, BUTTON_CLICKED
        double-click: BUTTON_PRESSED, BUTTON_RELEASED, BUTTON_PRESSED, BUTTON_RELEASED, BUTTON_DOUBLECLICKED
        long pressed: BUTTON_PRESSED, BUTTON_LONGPRESSED,  BUTTON_RELEASED
        @param listener: the listener function (with boolean parameter event) to register.
        @param enableClick: if True, the click/double-click is also registered (default: False)
        @param doubleClickTime: the time (in seconds) to wait for a double click (default: set in SharedContants)
        """
        if enableClick:
            global _xButtonListener, _doubleClickTime
            _doubleClickTime = doubleClickTime
            self.addButtonListener(_onXButtonEvent)
            _xButtonListener = listener
        else:
            global _buttonListener
            _buttonListener = listener

    def setButtonEnabled(self, enable):
        """
        Enables/disables the push button. The button is enabled, when the Robot is created.
        @param enable: if True, the button is enabled; otherwise disabled
        """
        Tools.debug("Calling setButtonEnabled() with enable = " + str(enable))
        global _isButtonEnabled
        _isButtonEnabled = enable

    def addBatteryMonitor(self, listener):
        """
        There is a small processor on the PCB (an STM8S003F3P6) which handles the voltage monitoring,
        as well as trying to reduce the impact of direct light on the IR sensors.
        It has 2 threshold voltages: At about 6.5V (3 consecutive readings) it flashes the red LED and
        disables the motor drivers. At about 6.2V it turns the red LED on permanently and
        sends a signal on GPIO24 pin 18 to the Pi. The software on the Pi can monitor this and
        shut down gracefully if required. If the voltage goes back above 7.0V then the system resets
        to Green LED and all enabled.

        Registers a listener function to get notifications when battery voltage is getting low.
        @param listener: the listener function (with no parameter) to register
        """
        batteryListener = listener

    # ---------------------------------------------- static methods -----------------------------------
    @staticmethod
    def getVersion():
        """
        @return: the module library version
        """
        return SharedConstants.VERSION

    @staticmethod
    def setSoundVolume(volume):
        """
        Sets the sound volume. Value is kept when the program exits.
        @param volume: the sound volume (0..100)
        """
        os.system("amixer sset PCM,0 " + str(volume) + "%  >/dev/null")

    @staticmethod
    def playTone(frequency, duration):
        """
        Plays a single sine tone with given frequency and duration.
        @param frequency: the frequency in Hz
        @param duration:  the duration in ms
        """
        os.system(
            "speaker-test -t sine -f "
            + str(frequency)
            + " >/dev/null & pid=$! ; sleep "
            + str(duration / 1000.0)
            + "s ; kill -9 $pid"
        )

    @staticmethod
    def getIPAddresses():
        """
        @return:  List of all IP addresses of machine
        """
        p = Popen(["ifconfig"], stdout=PIPE)
        ifc_resp = p.communicate()
        patt = re.compile(r"inet\s*\w*\S*:\s*(\d{1,3}\.\d{1,3}\.\d{1,3}\.\d{1,3})")
        resp = patt.findall(ifc_resp[0])
        return resp

    @staticmethod
    def initSound(soundFile, volume):
        """
        Prepares the given wav or mp3 sound file for playing with given volume (0..100). The sound
        sound channel is opened and a background noise is emitted.
        @param soundFile: the sound file in the local file system
        @volume: the sound volume (0..100)
        @returns: True, if successful; False if the sound system is not available or the sound file
        cannot be loaded
        """
        try:
            pygame.mixer.init()
        except:
            # print "Error while initializing sound system"
            return False
        try:
            pygame.mixer.music.load(soundFile)
        except:
            pygame.mixer.quit()
            # print "Error while loading sound file", soundFile
            return False
        try:
            pygame.mixer.music.set_volume(volume / 100.0)
        except:
            return False
        return True

    @staticmethod
    def closeSound():
        """
        Stops any playing sound and closes the sound channel.
        """
        try:
            pygame.mixer.stop()
            pygame.mixer.quit()
        except:
            pass

    @staticmethod
    def playSound():
        """
        Starts playing.
        """
        try:
            pygame.mixer.music.play()
        except:
            pass

    @staticmethod
    def fadeoutSound(time):
        """
        Decreases the volume slowly and stops playing.
        @param time: the fade out time in ms
        """
        try:
            pygame.mixer.music.fadeout(time)
        except:
            pass

    @staticmethod
    def setSoundVolume(volume):
        """
        Sets the volume while the sound is playing.
        @param volume: the sound volume (0..100)
        """
        try:
            pygame.mixer.music.set_volume(volume / 100.0)
        except:
            pass

    @staticmethod
    def stopSound():
        """
        Stops playing sound.
        """
        try:
            pygame.mixer.music.stop()
        except:
            pass

    @staticmethod
    def pauseSound():
        """
        Temporarily stops playing at current position.
        """
        try:
            pygame.mixer.music.pause()
        except:
            pass

    @staticmethod
    def resumeSound():
        """
        Resumes playing from stop position.
        """
        try:
            pygame.mixer.music.unpause()
        except:
            pass

    @staticmethod
    def rewindSound():
        """
        Resumes playing from the beginning.
        """
        try:
            pygame.mixer.music.rewind()
        except:
            pass

    @staticmethod
    def isSoundPlaying():
        """
        @return: True, if the sound is playing; otherwise False
        """
        try:
            return pygame.mixer.music.get_busy()
        except:
            return False
Exemple #9
0
flirZoom = 2

GPIO.setmode(GPIO.BCM)
PWM_OEpin = 4
GPIO.setup(PWM_OEpin, GPIO.OUT)

zoomInPin = 11
zoomOutPin = 9
GPIO.setup(zoomInPin, GPIO.OUT)
GPIO.setup(zoomOutPin, GPIO.OUT)
GPIO.output(zoomInPin, 0)
GPIO.output(zoomOutPin, 0)

pwm_frequency = 50
pwm_i2c_address = 0x40
pwm = PWM(SMBus(1), pwm_i2c_address)
pwm.setFreq(pwm_frequency)

print("Starting Lidar and Battery")
measThread = threading.Thread(target=getMeasurements)
measThread.daemon = True
measThread.start()

print("Starting Motor Control")
motorThread = threading.Thread(target=controlMotors)
motorThread.daemon = True
#motorThread.start()


def stopmotors():
Exemple #10
0
def setupSteereServo():
    global pwm
    bus = SMBus(1)
    pwm = PWM(bus, i2c_address)
    pwm.setFreq(fPWM)
 def setup(self):
     self.pwm = PWM(self.bus, self.i2c_address)
     self.pwm.setFreq(self.fPWM)
class SG90Dog:

    def __init__(self):
        self.state = 2
        self.fPWM = 50
        self.i2c_address = 0x40 # (standard) adapt to your module
        self.channel = 0 # adapt to your wiring
        self.a = 8.5 # adapt to your servo
        self.b = 2  # adapt to your servo

        self.bus = SMBus(3) # Raspberry Pi revision 2
        self.pwm = PWM(self.bus, self.i2c_address)
        self.pwm.setFreq(self.fPWM)

        self.setup()

        self.zeroBot()

        self.frLeg = Leg3d(side=1)
        self.flLeg = Leg3d(side=2)
        self.lrLeg = Leg3d(side=2)
        self.rrLeg = Leg3d(side=1)
        self.t = 0

        # walker = Walk3d(stride_height=-0.01,stride_length =.02)

        bpm = 138#music tempo
        self.freq = bpm/60.*2*pi
        self.amp = 0.015

    def setup(self):
        self.pwm = PWM(self.bus, self.i2c_address)
        self.pwm.setFreq(self.fPWM)

    def zeroBot(self):
        duty = self.a/180*90+self.b
        for ch in range(0,16):
            self.pwm.setDuty(ch,duty)

    def setHips(self,angle):
        duty = self.a / 180 * angle + self.b
        for ch in range(8,12):
            self.pwm.setDuty(ch,duty)

    def setLeg(self,femur,tibia,ch):
        fduty = self.a / 180 * femur + self.b
        tduty = self.a/180*tibia+self.b
        self.pwm.setDuty(ch,fduty)
        self.pwm.setDuty(ch+1,tduty)

    def setLeg3d(self,femur,tibia,hip,ch,hipch):
        fduty = self.a/180 * femur + self.b
        tduty = self.a/180*tibia+self.b
        hduty = self.a/180*hip+self.b
        # print fduty,tduty,hduty
        self.pwm.setDuty(ch,fduty)
        self.pwm.setDuty(ch+1,tduty)
        self.pwm.setDuty(hipch,hduty)


    def doTurn(self,freq,yamp,zamp,t):
        phifr = 0
        philr = 3*pi/2
        phifl = pi
        phirr = 1*pi/2
        yfl = -yamp*sin(freq*t+phifl)
        xfl = 0
        zfl = zamp*cos(freq*t+phifl)
        yfr = yamp*sin(freq*t+phifr)
        xfr = 0
        zfr = zamp*cos(freq*t+phifr)
        ylr = -yamp*sin(freq*t+philr)
        xlr = 0
        zlr = zamp*cos(freq*t+philr)
        yrr = yamp*sin(freq*t+phirr)
        xrr = 0
        zrr = zamp*cos(freq*t+phirr)
        return xfl,yfl,zfl,xfr,yfr,zfr,xlr,ylr,zlr,xrr,yrr,zrr

    def doWalk(self,freq,xamp,zamp,t):
        phifr = 0
        philr = pi/2
        phifl = pi
        phirr = 3*pi/2
        xfl = xamp*sin(freq*t+phifl)
        yfl = 0
        zfl = zamp*cos(freq*t+phifl)
        xfr = xamp*sin(freq*t+phifr)
        yfr = 0
        zfr = zamp*cos(freq*t+phifr)
        xlr = xamp*sin(freq*t+philr)
        ylr = 0
        zlr = zamp*cos(freq*t+philr)
        xrr = xamp*sin(freq*t+phirr)
        yrr = 0
        zrr = zamp*cos(freq*t+phirr)
        return xfl,yfl,zfl,xfr,yfr,zfr,xlr,ylr,zlr,xrr,yrr,zrr

    def doStand(self,freq,amp,amp2,t):
        xfl = 0#amp*sin(freq*t)
        yfl = 0
        zfl = 0
        xfr = .00#amp*sin(freq*t)
        yfr = 0
        zfr = 0
        xlr = 0#amp*sin(freq*t)
        ylr = 0
        zlr = 0
        xrr = 0#amp*sin(freq*t)
        yrr = 0
        zrr = 0
        return xfl,yfl,zfl,xfr,yfr,zfr,xlr,ylr,zlr,xrr,yrr,zrr

    def doDown(self,freq,amp,amp2,t):
        xfl = .01#amp*sin(freq*t)
        yfl = 0
        zfl = -.025
        xfr = .01#amp*sin(freq*t)
        yfr = 0
        zfr = -.025
        xlr = -.025#amp*sin(freq*t)
        ylr = 0
        zlr = -.025
        xrr = -.025#amp*sin(freq*t)
        yrr = 0
        zrr = -.025
        return xfl,yfl,zfl,xfr,yfr,zfr,xlr,ylr,zlr,xrr,yrr,zrr

    def doSit(self,freq,amp,amp2,t):
        xfl = .01#amp*sin(freq*t)
        yfl = 0
        zfl = .03
        xfr = .01#amp*sin(freq*t)
        yfr = 0
        zfr = .03
        xlr = -.025#amp*sin(freq*t)
        ylr = 0
        zlr = -.03
        xrr = -.025#amp*sin(freq*t)
        yrr = 0
        zrr = -.03
        return xfl,yfl,zfl,xfr,yfr,zfr,xlr,ylr,zlr,xrr,yrr,zrr


    def doSway(self,freq,amp,amp2,t):
        xfl = 0#amp*sin(freq*t)
        yfl = -amp*sin(.5*freq*t)
        zfl = 0
        xfr = 0#amp*sin(freq*t)
        yfr = amp*sin(.5*freq*t)
        zfr = 0
        xlr = 0#amp*sin(freq*t)
        ylr = amp*sin(.5*freq*t)
        zlr = 0
        xrr = 0#amp*sin(freq*t)
        yrr = -amp*sin(.5*freq*t)
        zrr = 0
        return xfl,yfl,zfl,xfr,yfr,zfr,xlr,ylr,zlr,xrr,yrr,zrr

    def doBump(self,freq,amp,amp2,t):
        xfl = 0#amp*sin(freq*t)
        yfl = 0
        zfl = amp*sin(freq*t)
        xfr = 0#amp*sin(freq*t)
        yfr = 0
        zfr = amp*sin(freq*t)
        xlr = 0#amp*sin(freq*t)
        ylr = 0
        zlr = amp*sin(freq*t)
        xrr = 0#amp*sin(freq*t)
        yrr = 0
        zrr = amp*sin(freq*t)
        return xfl,yfl,zfl,xfr,yfr,zfr,xlr,ylr,zlr,xrr,yrr,zrr

    def doStompL(self,freq,amp,amp2,t):
        xfl = 0#amp*sin(freq*t)
        yfl = amp*cos(.5*freq*t)
        zfl = amp*sin(freq*t)
        xfr = 0#amp*sin(freq*t)
        yfr = -2*amp
        zfr = amp
        xlr = -amp#amp*sin(freq*t)
        ylr = -2*amp
        zlr = 0
        xrr = -amp#amp*sin(freq*t)
        yrr = 2*amp
        zrr = 0
        return xfl,yfl,zfl,xfr,yfr,zfr,xlr,ylr,zlr,xrr,yrr,zrr


    def update(self,dt,action,freq,amp):
        self.t+=dt
        self.amp = amp
        self.freq = freq
        t = self.t
        if action=="stompleft":
            xfl,yfl,zfl,xfr,yfr,zfr,xlr,ylr,zlr,xrr,yrr,zrr = self.doStompL(freq,amp,amp,t)
        elif action== "bump":
            xfl,yfl,zfl,xfr,yfr,zfr,xlr,ylr,zlr,xrr,yrr,zrr = self.doBump(freq,amp,amp,t)
        elif action=="sway":
            xfl,yfl,zfl,xfr,yfr,zfr,xlr,ylr,zlr,xrr,yrr,zrr = self.doSway(freq,amp,amp,t)
        elif action=="sit":
            xfl,yfl,zfl,xfr,yfr,zfr,xlr,ylr,zlr,xrr,yrr,zrr = self.doSit(freq,amp,amp,t)
        elif action=="down":
            xfl,yfl,zfl,xfr,yfr,zfr,xlr,ylr,zlr,xrr,yrr,zrr = self.doDown(freq,amp,amp,t)
        elif action=="stand":
            xfl,yfl,zfl,xfr,yfr,zfr,xlr,ylr,zlr,xrr,yrr,zrr = self.doStand(freq,amp,amp,t)
        elif action=="walk":
            xfl,yfl,zfl,xfr,yfr,zfr,xlr,ylr,zlr,xrr,yrr,zrr = self.doWalk(freq,amp,amp,t)
        elif action=="turn":
            xfl,yfl,zfl,xfr,yfr,zfr,xlr,ylr,zlr,xrr,yrr,zrr = self.doTurn(freq,amp,amp,t)
        else:
            xfl,yfl,zfl,xfr,yfr,zfr,xlr,ylr,zlr,xrr,yrr,zrr = 0,0,0,0,0,0,0,0,0,0,0,0

        flfem,fltib,flhip = self.flLeg.servoAngles(xfl,yfl,zfl)
        frfem,frtib,frhip = self.frLeg.servoAngles(xfr,yfr,zfr)
        lrfem,lrtib,lrhip = self.lrLeg.servoAngles(xlr,ylr,zlr)
        rrfem,rrtib,rrhip = self.rrLeg.servoAngles(xrr,yrr,zrr)

        self.setLeg3d(frfem,frtib,frhip,0,8)
        self.setLeg3d(flfem,fltib,flhip,2,9)
        self.setLeg3d(lrfem,lrtib,lrhip,4,10)
        self.setLeg3d(rrfem,rrtib,rrhip,6,11)
Exemple #13
0
    def __init__(self, ipAddress="", buttonEvent=None):
        '''
        Creates an instance of MyRobot and initalizes the GPIO.
        '''
        if MyRobot._myInstance != None:
            raise Exception("Only one instance of MyRobot allowed")
        global _isButtonEnabled, _buttonListener

        _buttonListener = buttonEvent

        # Use physical pin numbers
        GPIO.setmode(GPIO.BOARD)
        GPIO.setwarnings(False)

        # Left motor
        GPIO.setup(SharedConstants.P_LEFT_FORWARD, GPIO.OUT)
        SharedConstants.LEFT_MOTOR_PWM[0] = GPIO.PWM(
            SharedConstants.P_LEFT_FORWARD, SharedConstants.MOTOR_PWM_FREQ)
        SharedConstants.LEFT_MOTOR_PWM[0].start(0)
        GPIO.setup(SharedConstants.P_LEFT_BACKWARD, GPIO.OUT)
        SharedConstants.LEFT_MOTOR_PWM[1] = GPIO.PWM(
            SharedConstants.P_LEFT_BACKWARD, SharedConstants.MOTOR_PWM_FREQ)
        SharedConstants.LEFT_MOTOR_PWM[1].start(0)

        # Right motor
        GPIO.setup(SharedConstants.P_RIGHT_FORWARD, GPIO.OUT)
        SharedConstants.RIGHT_MOTOR_PWM[0] = GPIO.PWM(
            SharedConstants.P_RIGHT_FORWARD, SharedConstants.MOTOR_PWM_FREQ)
        SharedConstants.RIGHT_MOTOR_PWM[0].start(0)
        GPIO.setup(SharedConstants.P_RIGHT_BACKWARD, GPIO.OUT)
        SharedConstants.RIGHT_MOTOR_PWM[1] = GPIO.PWM(
            SharedConstants.P_RIGHT_BACKWARD, SharedConstants.MOTOR_PWM_FREQ)
        SharedConstants.RIGHT_MOTOR_PWM[1].start(0)

        # IR sensors
        GPIO.setup(SharedConstants.P_FRONT_LEFT, GPIO.IN, GPIO.PUD_UP)
        GPIO.setup(SharedConstants.P_FRONT_CENTER, GPIO.IN, GPIO.PUD_UP)
        GPIO.setup(SharedConstants.P_FRONT_RIGHT, GPIO.IN, GPIO.PUD_UP)
        GPIO.setup(SharedConstants.P_LINE_LEFT, GPIO.IN, GPIO.PUD_UP)
        GPIO.setup(SharedConstants.P_LINE_RIGHT, GPIO.IN, GPIO.PUD_UP)

        # Establish event recognition from battery monitor
        GPIO.setup(SharedConstants.P_BATTERY_MONITOR, GPIO.IN, GPIO.PUD_UP)
        GPIO.add_event_detect(SharedConstants.P_BATTERY_MONITOR, GPIO.RISING,
                              _onBatteryDown)

        Tools.debug("Trying to detect I2C bus")
        self._bus = smbus.SMBus(1)  # For revision 2 Raspberry Pi
        Tools.debug("Found SMBus for revision 2")

        # I2C PWM chip PCM9685 for LEDs and servos
        self.pwm = PWM(self._bus, SharedConstants.PWM_I2C_ADDRESS)
        self.pwm.setFreq(SharedConstants.PWM_FREQ)
        self.isPCA9685Available = self.pwm._isAvailable
        if self.isPCA9685Available:
            # clear all LEDs
            for id in range(3):
                self.pwm.setDuty(3 * id, 0)
                self.pwm.setDuty(3 * id + 1, 0)
                self.pwm.setDuty(3 * id + 2, 0)
        # I2C analog extender chip
        Tools.debug("Trying to detect PCF8591P I2C expander")
        channel = 0
        try:
            self._bus.write_byte(SharedConstants.ADC_I2C_ADDRESS, channel)
            self._bus.read_byte(
                SharedConstants.ADC_I2C_ADDRESS)  # ignore reply
            data = self._bus.read_byte(SharedConstants.ADC_I2C_ADDRESS)
            Tools.debug("Found PCF8591P I2C expander")
        except:
            Tools.debug("PCF8591P I2C expander not found")

        self.displayType = "none"
        Tools.debug("Trying to detect OLED display")
        self.oled = OLED1306()
        if self.oled.isDeviceAvailable():
            self.displayType = "oled"
        else:
            Tools.debug("'oled' display not found")
            self.oled = None
            Tools.debug("Trying to detect 7-segment display")
            try:
                addr = 0x20
                rc = self._bus.read_byte_data(addr, 0)
                if rc != 0xA0:  # returns 255 for 4tronix
                    raise Exception()
                Tools.delay(100)
                self.displayType = "didel1"
            except:
                Tools.debug("'didel1' display not found")
            if self.displayType == "none":
                try:
                    addr = 0x20
                    self._bus.write_byte_data(
                        addr, 0x00, 0x00)  # Set all of bank 0 to outputs
                    Tools.delay(100)
                    self.displayType = "4tronix"
                except:
                    Tools.debug("'4tronix' display not found")
            if self.displayType == "none":
                try:
                    addr = 0x24  # old dgTell from didel
                    data = [0] * 4
                    self._bus.write_i2c_block_data(
                        addr, data[0], data[1:])  # trying to clear display
                    self.displayType = "didel"
                except:
                    Tools.debug("'didel (old type)' display not found")

        Tools.debug("Display type '" + self.displayType + "'")

        # Initializing (clear) display, if available
        if self.displayType == "4tronix":
            Disp4tronix().clear()
        elif self.displayType == "didel":
            DgTell().clear()
        elif self.displayType == "didel1":
            DgTell1().clear()

        GPIO.setup(SharedConstants.P_BUTTON, GPIO.IN, GPIO.PUD_UP)
        # Establish event recognition from button event
        GPIO.add_event_detect(SharedConstants.P_BUTTON, GPIO.BOTH,
                              _onButtonEvent)
        _isButtonEnabled = True
        Tools.debug("MyRobot instance created. Lib Version: " +
                    SharedConstants.VERSION)
        self.sensorThread = None
        MyRobot._myInstance = self
Exemple #14
0
class MyRobot(object):
    '''
    Singleton class that represents a robot.
    Signature for the butten event callback: buttonEvent(int).
    (BUTTON_PRESSED, BUTTON_RELEASED, BUTTON_LONGPRESSED defined in ShareConstants.)
    @param ipAddress the IP address (default: None for autonomous mode)
    @param buttonEvent the callback function for pushbutton events (default: None)
    '''
    _myInstance = None

    def __init__(self, ipAddress="", buttonEvent=None):
        '''
        Creates an instance of MyRobot and initalizes the GPIO.
        '''
        if MyRobot._myInstance != None:
            raise Exception("Only one instance of MyRobot allowed")
        global _isButtonEnabled, _buttonListener

        _buttonListener = buttonEvent

        # Use physical pin numbers
        GPIO.setmode(GPIO.BOARD)
        GPIO.setwarnings(False)

        # Left motor
        GPIO.setup(SharedConstants.P_LEFT_FORWARD, GPIO.OUT)
        SharedConstants.LEFT_MOTOR_PWM[0] = GPIO.PWM(
            SharedConstants.P_LEFT_FORWARD, SharedConstants.MOTOR_PWM_FREQ)
        SharedConstants.LEFT_MOTOR_PWM[0].start(0)
        GPIO.setup(SharedConstants.P_LEFT_BACKWARD, GPIO.OUT)
        SharedConstants.LEFT_MOTOR_PWM[1] = GPIO.PWM(
            SharedConstants.P_LEFT_BACKWARD, SharedConstants.MOTOR_PWM_FREQ)
        SharedConstants.LEFT_MOTOR_PWM[1].start(0)

        # Right motor
        GPIO.setup(SharedConstants.P_RIGHT_FORWARD, GPIO.OUT)
        SharedConstants.RIGHT_MOTOR_PWM[0] = GPIO.PWM(
            SharedConstants.P_RIGHT_FORWARD, SharedConstants.MOTOR_PWM_FREQ)
        SharedConstants.RIGHT_MOTOR_PWM[0].start(0)
        GPIO.setup(SharedConstants.P_RIGHT_BACKWARD, GPIO.OUT)
        SharedConstants.RIGHT_MOTOR_PWM[1] = GPIO.PWM(
            SharedConstants.P_RIGHT_BACKWARD, SharedConstants.MOTOR_PWM_FREQ)
        SharedConstants.RIGHT_MOTOR_PWM[1].start(0)

        # IR sensors
        GPIO.setup(SharedConstants.P_FRONT_LEFT, GPIO.IN, GPIO.PUD_UP)
        GPIO.setup(SharedConstants.P_FRONT_CENTER, GPIO.IN, GPIO.PUD_UP)
        GPIO.setup(SharedConstants.P_FRONT_RIGHT, GPIO.IN, GPIO.PUD_UP)
        GPIO.setup(SharedConstants.P_LINE_LEFT, GPIO.IN, GPIO.PUD_UP)
        GPIO.setup(SharedConstants.P_LINE_RIGHT, GPIO.IN, GPIO.PUD_UP)

        # Establish event recognition from battery monitor
        GPIO.setup(SharedConstants.P_BATTERY_MONITOR, GPIO.IN, GPIO.PUD_UP)
        GPIO.add_event_detect(SharedConstants.P_BATTERY_MONITOR, GPIO.RISING,
                              _onBatteryDown)

        Tools.debug("Trying to detect I2C bus")
        self._bus = smbus.SMBus(1)  # For revision 2 Raspberry Pi
        Tools.debug("Found SMBus for revision 2")

        # I2C PWM chip PCM9685 for LEDs and servos
        self.pwm = PWM(self._bus, SharedConstants.PWM_I2C_ADDRESS)
        self.pwm.setFreq(SharedConstants.PWM_FREQ)
        self.isPCA9685Available = self.pwm._isAvailable
        if self.isPCA9685Available:
            # clear all LEDs
            for id in range(3):
                self.pwm.setDuty(3 * id, 0)
                self.pwm.setDuty(3 * id + 1, 0)
                self.pwm.setDuty(3 * id + 2, 0)
        # I2C analog extender chip
        Tools.debug("Trying to detect PCF8591P I2C expander")
        channel = 0
        try:
            self._bus.write_byte(SharedConstants.ADC_I2C_ADDRESS, channel)
            self._bus.read_byte(
                SharedConstants.ADC_I2C_ADDRESS)  # ignore reply
            data = self._bus.read_byte(SharedConstants.ADC_I2C_ADDRESS)
            Tools.debug("Found PCF8591P I2C expander")
        except:
            Tools.debug("PCF8591P I2C expander not found")

        self.displayType = "none"
        Tools.debug("Trying to detect OLED display")
        self.oled = OLED1306()
        if self.oled.isDeviceAvailable():
            self.displayType = "oled"
        else:
            Tools.debug("'oled' display not found")
            self.oled = None
            Tools.debug("Trying to detect 7-segment display")
            try:
                addr = 0x20
                rc = self._bus.read_byte_data(addr, 0)
                if rc != 0xA0:  # returns 255 for 4tronix
                    raise Exception()
                Tools.delay(100)
                self.displayType = "didel1"
            except:
                Tools.debug("'didel1' display not found")
            if self.displayType == "none":
                try:
                    addr = 0x20
                    self._bus.write_byte_data(
                        addr, 0x00, 0x00)  # Set all of bank 0 to outputs
                    Tools.delay(100)
                    self.displayType = "4tronix"
                except:
                    Tools.debug("'4tronix' display not found")
            if self.displayType == "none":
                try:
                    addr = 0x24  # old dgTell from didel
                    data = [0] * 4
                    self._bus.write_i2c_block_data(
                        addr, data[0], data[1:])  # trying to clear display
                    self.displayType = "didel"
                except:
                    Tools.debug("'didel (old type)' display not found")

        Tools.debug("Display type '" + self.displayType + "'")

        # Initializing (clear) display, if available
        if self.displayType == "4tronix":
            Disp4tronix().clear()
        elif self.displayType == "didel":
            DgTell().clear()
        elif self.displayType == "didel1":
            DgTell1().clear()

        GPIO.setup(SharedConstants.P_BUTTON, GPIO.IN, GPIO.PUD_UP)
        # Establish event recognition from button event
        GPIO.add_event_detect(SharedConstants.P_BUTTON, GPIO.BOTH,
                              _onButtonEvent)
        _isButtonEnabled = True
        Tools.debug("MyRobot instance created. Lib Version: " +
                    SharedConstants.VERSION)
        self.sensorThread = None
        MyRobot._myInstance = self

    def registerSensor(self, sensor):
        if self.sensorThread == None:
            self.sensorThread = SensorThread()
            self.sensorThread.start()
        self.sensorThread.add(sensor)

    def exit(self):
        """
        Cleans-up and releases all resources.
        """
        global _isButtonEnabled
        Tools.debug("Calling Robot.exit()")
        self.setButtonEnabled(False)

        # Stop motors
        SharedConstants.LEFT_MOTOR_PWM[0].ChangeDutyCycle(0)
        SharedConstants.LEFT_MOTOR_PWM[1].ChangeDutyCycle(0)
        SharedConstants.RIGHT_MOTOR_PWM[0].ChangeDutyCycle(0)
        SharedConstants.RIGHT_MOTOR_PWM[1].ChangeDutyCycle(0)

        # Stop button thread, if necessary
        if _buttonThread != None:
            _buttonThread.stop()

        # Stop display
        display = Display._myInstance
        if display != None:
            display.stopTicker()
            display.clear()

        if self.isPCA9685Available:
            Led.clearAll()
        MyRobot.closeSound()

        if self.sensorThread != None:
            self.sensorThread.stop()
            self.sensorThread.join(2000)

#       GPIO.cleanup()
# Do not cleanup, otherwise button will not work any more when coming back from remote execution

        Tools.delay(2000)  # avoid "sys.excepthook is missing"

    def isButtonDown(self):
        '''
        Checks if button is currently pressed.
        @return: True, if the button is actually pressed
        '''
        Tools.delay(1)
        return GPIO.input(SharedConstants.P_BUTTON) == GPIO.LOW

    def isButtonHit(self):
        '''
        Checks, if the button was ever hit or hit since the last invocation.
        @return: True, if the button was hit; otherwise False
        '''
        global _isBtnHit
        Tools.delay(1)
        hit = _isBtnHit
        _isBtnHit = False
        return hit

    def isEscapeHit(self):
        '''
        Same as isButtonHit() for compatibility with remote mode.
        '''
        return self.isButtonHit()

    def isEnterHit(self):
        '''
        Empty method for compatibility with remote mode.
        '''
        pass

    def isUpHit(self):
        '''
        Empty method for compatibility with remote mode.
        '''
        pass

    def isDownHit(self):
        '''
        Empty method for compatibility with remote mode.
        '''
        pass

    def isLeftHit(self):
        '''
        Empty method for compatibility with remote mode.
        '''
        pass

    def isRightHit(self):
        '''
        Empty method for compatibility with remote mode.
        '''
        pass

    def addButtonListener(
            self,
            listener,
            enableClick=False,
            doubleClickTime=SharedConstants.BUTTON_DOUBLECLICK_TIME):
        '''
        Registers a listener function to get notifications when the pushbutton is pressed, released or long pressed.
        If enableClick = True, in addition click and double-click events are reported. The click event not immediately
        reported, but only if within the doubleClickTime no other click is gererated.
        The value are defined as ShareConstants.BUTTON_PRESSED, ShareConstants.BUTTON_LONGPRESSED, ShareConstants.BUTTON_RELEASED,
        ShareConstants.BUTTON_CLICKED, ShareConstants.BUTTON_DOUBLECLICKED.
        With enableClick = False and the button is long pressed and released the sequence is: BUTTON_PRESSED, BUTTON_LONGPRESSED, BUTTON_RELEASED.
        With enableClick = True the sequences are the following:
        click: BUTTON_PRESSED, BUTTON_RELEASED, BUTTON_CLICKED
        double-click: BUTTON_PRESSED, BUTTON_RELEASED, BUTTON_PRESSED, BUTTON_RELEASED, BUTTON_DOUBLECLICKED
        long pressed: BUTTON_PRESSED, BUTTON_LONGPRESSED,  BUTTON_RELEASED
        @param listener: the listener function (with boolean parameter event) to register.
        @param enableClick: if True, the click/double-click is also registered (default: False)
        @param doubleClickTime: the time (in seconds) to wait for a double click (default: set in SharedContants)
        '''
        if enableClick:
            global _xButtonListener, _doubleClickTime
            _doubleClickTime = doubleClickTime
            self.addButtonListener(_onXButtonEvent)
            _xButtonListener = listener
        else:
            global _buttonListener
            _buttonListener = listener

    def setButtonEnabled(self, enable):
        '''
        Enables/disables the push button. The button is enabled, when the Robot is created.
        @param enable: if True, the button is enabled; otherwise disabled
        '''
        Tools.debug("Calling setButtonEnabled() with enable = " + str(enable))
        global _isButtonEnabled
        _isButtonEnabled = enable

    def addBatteryMonitor(self, listener):
        '''
        There is a small processor on the PCB (an STM8S003F3P6) which handles the voltage monitoring,
        as well as trying to reduce the impact of direct light on the IR sensors.
        It has 2 threshold voltages: At about 6.5V (3 consecutive readings) it flashes the red LED and
        disables the motor drivers. At about 6.2V it turns the red LED on permanently and
        sends a signal on GPIO24 pin 18 to the Pi. The software on the Pi can monitor this and
        shut down gracefully if required. If the voltage goes back above 7.0V then the system resets
        to Green LED and all enabled.

        Registers a listener function to get notifications when battery voltage is getting low.
        @param listener: the listener function (with no parameter) to register
        '''
        batteryListener = listener

# ---------------------------------------------- static methods -----------------------------------

    @staticmethod
    def getVersion():
        '''
        @return: the module library version
        '''
        return SharedConstants.VERSION

    @staticmethod
    def setSoundVolume(volume):
        '''
        Sets the sound volume. Value is kept when the program exits.
        @param volume: the sound volume (0..100)
        '''
        os.system("amixer sset PCM,0 " + str(volume) + "%  >/dev/null")

    @staticmethod
    def playTone(frequency, duration):
        '''
        Plays a single sine tone with given frequency and duration.
        @param frequency: the frequency in Hz
        @param duration:  the duration in ms
        '''
        os.system("speaker-test -t sine -f " + str(frequency) +
                  " >/dev/null & pid=$! ; sleep " + str(duration / 1000.0) +
                  "s ; kill -9 $pid")

    @staticmethod
    def getIPAddresses():
        '''
        @return:  List of all IP addresses of machine
        '''
        ip = []
        ifname = "wlan0"
        try:
            s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
            ip.append(
                socket.inet_ntoa(
                    fcntl.ioctl(
                        s.fileno(),
                        0x8915,  # SIOCGIFADDR
                        struct.pack('256s', ifname[:15]))[20:24]))
        except:
            pass
        print "Got IP address:", ip
        return ip

    @staticmethod
    def initSound(soundFile, volume):
        '''
        Prepares the given wav or mp3 sound file for playing with given volume (0..100). The sound
        sound channel is opened and a background noise is emitted.
        @param soundFile: the sound file in the local file system
        @volume: the sound volume (0..100)
        @returns: True, if successful; False if the sound system is not available or the sound file
        cannot be loaded
        '''
        try:
            pygame.mixer.init()
        except:
            # print "Error while initializing sound system"
            return False
        try:
            pygame.mixer.music.load(soundFile)
        except:
            pygame.mixer.quit()
            # print "Error while loading sound file", soundFile
            return False
        try:
            pygame.mixer.music.set_volume(volume / 100.0)
        except:
            return False
        return True

    @staticmethod
    def closeSound():
        '''
        Stops any playing sound and closes the sound channel.
        '''
        try:
            pygame.mixer.stop()
            pygame.mixer.quit()
        except:
            pass

    @staticmethod
    def playSound():
        '''
        Starts playing.
        '''
        try:
            pygame.mixer.music.play()
        except:
            pass

    @staticmethod
    def fadeoutSound(time):
        '''
        Decreases the volume slowly and stops playing.
        @param time: the fade out time in ms
        '''
        try:
            pygame.mixer.music.fadeout(time)
        except:
            pass

    @staticmethod
    def setSoundVolume(volume):
        '''
        Sets the volume while the sound is playing.
        @param volume: the sound volume (0..100)
        '''
        try:
            pygame.mixer.music.set_volume(volume / 100.0)
        except:
            pass

    @staticmethod
    def stopSound():
        '''
        Stops playing sound.
        '''
        try:
            pygame.mixer.music.stop()
        except:
            pass

    @staticmethod
    def pauseSound():
        '''
        Temporarily stops playing at current position.
        '''
        try:
            pygame.mixer.music.pause()
        except:
            pass

    @staticmethod
    def resumeSound():
        '''
        Resumes playing from stop position.
        '''
        try:
            pygame.mixer.music.unpause()
        except:
            pass

    @staticmethod
    def rewindSound():
        '''
        Resumes playing from the beginning.
        '''
        try:
            pygame.mixer.music.rewind()
        except:
            pass

    @staticmethod
    def isSoundPlaying():
        '''
        @return: True, if the sound is playing; otherwise False
        '''
        try:
            return pygame.mixer.music.get_busy()
        except:
            return False
Exemple #15
0
 def __init__(self):
     global pwm
     bus = SMBus(1)  # Raspberry Pi revision 2
     pwm = PWM(bus, self.i2c_address)
     pwm.setFreq(self.fPWM)
class WheelCalibration(object):
    '''
    This class is made to use ESC device in Raspberry Pi.
    Becareful when you connect ESC to your Raspberry Pi board.
    It needs very large current, it can make broken your circuit.

    '''
    def __init__(self, drive, steer, bus_number=None, address=0x40, frequency = 60, drive_ch = 0, steer_ch = 1):
        ''' This is very important part of set ESC.
        If you want to drive motor by this source, you have to use pca9685 drive.
        Because the ESC circuit is connected with PCA9685 PWM circuit board's channel pins

        Argument
        bus_number : bus type of raspberry Pi. If it doesn't set, pca9685 module set value as default.
        address : I2C slave address
        frequency : driving motor(forward/backward motor) PWM frequency.
        driver : pca9685 channel number of driving motor
        steer : pca9685 channel number of steering motor
        '''

        #mode - 0 : forward, 1 : backward, 2 : Neutral, 3 : None
        self.set_mode = 3
        self.steer_value_loaded = False
        self.drive_value_loaded = False
        self.current_mode = ["Forward", "Backward", "Neutral", "None"]

        #Set Channel
        self.drive_ch = drive_ch 
        self.steer_ch = steer_ch

        #Default values for steer
        #self.steer_NEUTRAL = 390 
        self.steer_center = steer
        self.steer_MIN = 280
        self.steer_MAX = 500
        self.steer_val = 0
        self.steer_diff = 15

        #Default values for drive(engine)
        #self.drive_NEUTRAL = 369 
        self.drive_neutral = drive 
        self.drive_MIN = 280
        self.drive_MAX = 450
        #self.drive_val = self.drive_neutral
        self.speed_forward = 0
        self.speed_backward = 0
        self.drive_diff = 5

        # PWM init
        self.pwm = PWM(bus_number, address)
        self.pwm.frequency = frequency
        self.pwm.setup()
        time.sleep(0.1)
        self.pwm.write(self.drive_ch,0,0)
        self.pwm.write(self.steer_ch,0,0)

    """
    def left(self):
        if(self.steer_value_loaded == True):
            if self.steer_val < self.steer_MAX:
                self.steer_val += self.steer_diff
                if self.steer_val > self.steer_MAX:
                    self.steer_val = self.steer_MAX
        self.pwm.write(self.steer_ch,0,self.steer_val)
        
    def right(self):
        if(self.steer_value_loaded == True):
            if self.steer_val > self.steer_MIN:
                self.steer_val -= self.steer_diff
                if self.steer_val < self.steer_MIN :
                    self.steer_val = self.steer_MIN
        self.pwm.write(self.steer_ch,0,self.steer_val)
    """
    def left(self):
        if(self.steer_value_loaded == True):
            self.steer_val += self.steer_diff

            if self.steer_val >= self.steer_MAX:
                self.steer_val = self.steer_MAX
            else:                
                self.pwm.write(self.steer_ch,0,self.steer_val)
        
    def right(self):
        if(self.steer_value_loaded == True):
            self.steer_val -= self.steer_diff

            if self.steer_val <= self.steer_MIN :
                self.steer_val = self.steer_MIN
            else:
                self.pwm.write(self.steer_ch,0,self.steer_val)


    def set_steer_center(self):
        self.steer_val = self.steer_center
        self.pwm.write(self.steer_ch,0,self.steer_val)
        time.sleep(0.1)
        self.pwm.write(self.steer_ch,0,0)
        self.steer_value_loaded = True

    def increase_speed(self):
        if(self.set_mode == 0): #if forward
            if self.speed_forward < self.drive_MAX:
                self.speed_forward += self.drive_diff
                if self.speed_forward > self.drive_MAX:
                    self.speed_forward = self.drive_MAX
            self.pwm.write(self.drive_ch, 0, self.speed_forward)        
        elif(self.set_mode == 1): #if backward
            if self.speed_backward > self.drive_MIN:
                self.speed_backward -= self.drive_diff
                if self.speed_backward < self.drive_MIN :
                    self.speed_backward = self.drive_MIN
            self.pwm.write(self.drive_ch, 0, self.speed_backward)        
        else:
            pass
        

    def decrease_speed(self):
        if(self.set_mode == 0): #if forward
            """
            if self.speed_forward > self.drive_neutral:
                self.speed_forward -= self.drive_diff
                if self.speed_forward < self.drive_neutral:
                    self.speed_forward = self.drive_neutral
            """
            if self.speed_forward > self.drive_MIN:
                self.speed_forward -= self.drive_diff
                                    
            self.pwm.write(self.drive_ch, 0, self.speed_forward)

        elif(self.set_mode == 1): #if backward
            """
            if self.speed_backward < self.drive_neutral:
                self.speed_backward += self.drive_diff
                if self.speed_backward > self.drive_neutral :
                    self.speed_backward = self.drive_neutral
            """
            if self.speed_backward < self.drive_MAX:
                self.speed_backward += self.drive_diff        
            self.pwm.write(self.drive_ch, 0, self.speed_backward)                    
        else:
            pass

    def set_drive_forward(self):
        if(self.drive_value_loaded == True):
            if(self.set_mode == 2 or self.set_mode == 3):
                self.set_mode = 0

    def set_drive_backward(self):
        if(self.drive_value_loaded == True):
            if(self.set_mode == 2 or self.set_mode == 3):
                self.set_mode = 1

    def set_drive_neutral(self):
        self.set_mode = 2
        self.pwm.write(self.drive_ch, 0,self.drive_neutral)
        self.speed_forward = self.drive_neutral
        self.speed_backward = self.drive_neutral
        self.drive_value_loaded = True
    
    """
    def set_speed_manually(self,speed):
        self.pwm.write(self.drive_ch, 0,speed)

    def set_steer_manually(self,steer):
        self.pwm.write(self.drive_ch, 0,steer)
    """
    
    def pwm_off(self):
        self.pwm.write(self.drive_ch, 0,0)
        self.pwm.write(self.steer_ch, 0,0)
        self.set_mode = 3
Exemple #17
0
class ESC(object):
    '''
    This class is made to use ESC device in Raspberry Pi.
    Becareful when you connect ESC to your Raspberry Pi board.
    It needs very large current, it can make broken your circuit.

    '''
    def __init__(self,
                 bus_number=None,
                 address=0x40,
                 frequency=60,
                 drive=0,
                 steer=1):
        ''' This is very important part of set ESC.
        If you want to drive motor by this source, you have to use pca9685 drive.
        Because the ESC circuit is connected with PCA9685 PWM circuit board's channel pins

        Argument
        bus_number : bus type of raspberry Pi. If it doesn't set, pca9685 module set value as default.
        address : I2C slave address
        frequency : driving motor(forward/backward motor) PWM frequency.
        driver : pca9685 channel number of driving motor
        steer : pca9685 channel number of steering motor
        '''
        self.pwm = PWM(bus_number, address)
        self.pwm.frequency = frequency
        self.pwm.setup()
        self.drive = drive
        self.steer = steer
        self.steer_NEUTRAL = 390  #default value. It needs to calibrate
        self.steer_MIN = 280
        self.steer_MAX = 500
        self.drive_NEUTRAL = 390  #default value. It needs to calibrate
        self.drive_MIN = 280
        self.drive_MAX = 450
        self.steer_val = self.steer_NEUTRAL
        self.drive_val = self.drive_NEUTRAL
        self.speed_forward = self.steer_NEUTRAL
        self.speed_backward = self.steer_NEUTRAL
        self.steer_diff = 35
        self.drive_diff = 5
        self.is_stop = True

    def calibrate_drive_NEUTRAL(self, cal_value=390):
        if cal_value > self.drive_MAX or cal_value < self.drive_MIN:
            print "Calibration value Fail"
        else:
            self.drive_NEUTRAL = cal_value

    def calibrate_steer_NEUTRAL(self, cal_value=390):
        if cal_value > self.steer_MAX or cal_value < self.steer_MIN:
            print "Stree Calibration value Fail"
        else:
            self.steer_NEUTRAL = cal_value

    def set_steer_strength(self, strength=35):
        if strength > (self.steer_MAX - self.steer_NEUTRAL):
            strength = self.steer_MAX - self.steer_NEUTRAL
        elif strength < (self.steer_NEUTRAL - self.steer_NEUTRAL):
            strength = self.steer_NEUTRAL - self.steer_NEUTRAL
        self.steer_diff = strength

    def left(self):
        if self.steer_val < self.steer_MAX:
            self.steer_val += self.steer_diff
            if self.steer_val > self.steer_MAX:
                self.steer_val = self.steer_MAX

        self.pwm.write(self.steer, 0, self.steer_val)

    def right(self):
        if self.steer_val > self.steer_MIN:
            self.steer_val -= self.steer_diff
            if self.steer_val < self.steer_MIN:
                self.steer_val = self.steer_MIN
        self.pwm.write(self.steer, 0, self.steer_val)

    def center(self):
        self.steer_val = self.steer_NEUTRAL
        self.pwm.write(self.steer, 0, self.steer_val)
        time.sleep(0.1)
        self.pwm.write(self.steer, 0, 0)

    def increase_speed(self):
        if self.speed_forward < self.drive_MAX:
            self.speed_forward += self.drive_diff
            if self.speed_forward > self.drive_MAX:
                self.speed_forward = self.drive_MAX

        if self.speed_backward > self.drive_MIN:
            self.speed_backward -= self.drive_diff
            if self.speed_backward < self.drive_MIN:
                self.speed_backward = self.drive_MIN
        print "set_speed === fwd : ", self.speed_forward, "bwd : ", self.speed_backward

    def decrease_speed(self):
        if self.speed_forward > self.drive_NEUTRAL:
            self.speed_forward -= self.drive_diff
            if self.speed_forward < self.drive_NEUTRAL:
                self.speed_forward = self.drive_NEUTRAL

        if self.speed_backward < self.drive_NEUTRAL:
            self.speed_backward += self.drive_diff
            if self.speed_backward > self.drive_NEUTRAL:
                self.speed_backward = self.drive_NEUTRAL
        print "set_speed === fwd : ", self.speed_forward, "bwd : ", self.speed_backward

    def set_speed(self, speed_val=1):
        '''
        speed_val : it should set number between 1 to 12
        '''
        if speed_val > 12:
            speed_val = 12
        self.speed_forward = self.drive_NEUTRAL + self.drive_diff * speed_val
        self.speed_backward = self.drive_NEUTRAL - self.drive_diff * speed_val
        print "set_speed === fwd : ", self.speed_forward, "bwd : ", self.speed_backward

    def forward(self):
        if self.is_stop:
            self.pwm.write(self.drive, 0, self.steer_NEUTRAL)
            time.sleep(0.1)
            self.is_stop = False
        self.pwm.write(self.drive, 0, self.speed_forward)

    def backward(self):
        if self.is_stop:
            self.pwm.write(self.drive, 0, self.steer_NEUTRAL)
            time.sleep(0.1)
            self.is_stop = False
        self.pwm.write(self.drive, 0, self.speed_backward)

    def stop(self):
        self.pwm.write(self.drive, 0, 0)
        self.is_stop = True
Exemple #18
0
def setup():
    global pwm
    # Raspberry Pi revision 2
    bus = SMBus(1)
    pwm = PWM(bus, i2c_address)
    pwm.setFreq(fPWM)
#!usr/bin/env python
# -*- coding:utf-8 -*-
# _*_ coding:cp1254 _*_
#Ahmed Demirezen Feezx1
import socket
import RPi.GPIO as gpio
import time
from PCA9685 import PWM
from smbus import SMBus

#gpio ayarları
gpio.setmode(gpio.BCM)
#PCA9685 setup
i2c_adres = 0x40
bus = SMBus(1)
pwm_s = PWM(bus, i2c_adres)
pwm_s.setFreq(50)
#step
gpio.setup(4, gpio.OUT)
gpio.setup(17, gpio.OUT)
gpio.setup(23, gpio.OUT)
gpio.setup(24, gpio.OUT)
#dc
gpio.setup(26, gpio.OUT)
gpio.setup(19, gpio.OUT)

gpio.output(26, gpio.LOW)
gpio.output(19, gpio.HIGH)
pwm_s.setDuty(2, 0)

#Server bilgileri(Çalıştırmadan önce bilgileri güncelleyin)
Exemple #20
0
import time
import smbus
from PCA9685 import PWM
bus = smbus.SMBus(1)
channel = 0
frequency = 50
address = 0x40
pwm = PWM(bus, address)
pwm.setFreq(frequency)
pwm.setDuty(channel, 2)
time.sleep(3)
while 1:
    for i in range(2, 13, 1):
        pwm.setDuty(channel, i)
        time.sleep(0.3)
    for i in range(12, 1, -1):
        pwm.setDuty(channel, i)
        time.sleep(0.3)
Exemple #21
0
def setup():
    global pwm
    bus = SMBus(1) # Raspberry Pi revision 2
    pwm = PWM(bus, i2c_address)
    pwm.setFreq(fPWM)