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
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
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)
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
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
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
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():
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)
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
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
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
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
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)
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)
def setup():
global pwm
bus = SMBus(1) # Raspberry Pi revision 2
pwm = PWM(bus, i2c_address)
pwm.setFreq(fPWM)
