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)
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
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)
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) host = "192.168.1.198" port = 8585 buf = 1024 s.connect((host, port)) while True: data = s.recv(buf) if data == "komut": print "Baglanti Basarili" #step motor elif data == "dal":
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