Пример #1
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)
Пример #2
0
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():

    print('Stopping Motors')
Пример #3
0
def setup():
    global pwm
    bus = SMBus(1)  # Raspberry Pi revision 2
    pwm = PWM(bus, i2c_address)
    pwm.setFreq(fPWM)
Пример #4
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
Пример #5
0
def setupSteereServo():
    global pwm
    bus = SMBus(1)
    pwm = PWM(bus, i2c_address)
    pwm.setFreq(fPWM)
# -*- 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)
s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
Пример #7
0
 def __init__(self):
     global pwm
     bus = SMBus(1)  # Raspberry Pi revision 2
     pwm = PWM(bus, self.i2c_address)
     pwm.setFreq(self.fPWM)
Пример #8
0
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)
Пример #9
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
Пример #10
0
def setup():
    global pwm
    bus = SMBus(1) # Raspberry Pi revision 2
    pwm = PWM(bus, i2c_address)
    pwm.setFreq(fPWM)