def init():
global p, q, a, b, pwm, pcfADC
#use physical pin numbering
GPIO.setmode(GPIO.BOARD)
#Set up IR obstacle sensors as inputs
GPIO.setup(irFL, GPIO.IN) # Left obstacle sensor
GPIO.setup(irFR, GPIO.IN) # Right obstacle sensor
GPIO.setup(irMID, GPIO.IN) # Centre Front obstacle sensor
#use pwm on inputs so motors don't go too fast
GPIO.setup(L1, GPIO.OUT)
p = GPIO.PWM(L1, 20)
p.start(0)
GPIO.setup(L2, GPIO.OUT)
q = GPIO.PWM(L2, 20)
q.start(0)
GPIO.setup(R1, GPIO.OUT)
a = GPIO.PWM(R1, 20)
a.start(0)
GPIO.setup(R2, GPIO.OUT)
b = GPIO.PWM(R2, 20)
b.start(0)
# Initalise the ADC
pcfADC = None # ADC object
try:
pcfADC = sgh_PCF8591P(1) #i2c, 0x48)
except:
PGType = PGLite
def init():
global p, q, a, b, pwm, pcfADC, PGType
PGType = PGFull
# Initialise the PCA9685 PWM device using the default address
try:
pwm = PWM(0x40, debug = False)
pwm.setPWMFreq(60) # Set frequency to 60 Hz
except:
PGType = PGLite # No PCA9685 so set to Pi2Go-Lite
#use physical pin numbering
GPIO.setmode(GPIO.BOARD)
#set up digital line detectors as inputs
GPIO.setup(lineRight, GPIO.IN) # Right line sensor
GPIO.setup(lineLeft, GPIO.IN) # Left line sensor
#Set up IR obstacle sensors as inputs
GPIO.setup(irFL, GPIO.IN) # Left obstacle sensor
GPIO.setup(irFR, GPIO.IN) # Right obstacle sensor
GPIO.setup(irMID, GPIO.IN) # Centre Front obstacle sensor
#use pwm on inputs so motors don't go too fast
GPIO.setup(L1, GPIO.OUT)
p = GPIO.PWM(L1, 20)
p.start(0)
GPIO.setup(L2, GPIO.OUT)
q = GPIO.PWM(L2, 20)
q.start(0)
GPIO.setup(R1, GPIO.OUT)
a = GPIO.PWM(R1, 20)
a.start(0)
GPIO.setup(R2, GPIO.OUT)
b = GPIO.PWM(R2, 20)
b.start(0)
# Initalise the ADC
pcfADC = None # ADC object
try:
pcfADC = sgh_PCF8591P(1) #i2c, 0x48)
except:
PGType = PGLite
# initialise servos (Pi2Go-Lite only)
if PGType == PGLite:
startServos()
#set up Pi2Go-Lite White LEDs as outputs
GPIO.setup(frontLED, GPIO.OUT)
GPIO.setup(rearLED, GPIO.OUT)
#set switch as input with pullup
if PGType == PGLite:
GPIO.setup(Lswitch, GPIO.IN, pull_up_down=GPIO.PUD_UP)
else:
GPIO.setup(switch, GPIO.IN, pull_up_down=GPIO.PUD_UP)
def init():
global p, q, a, b, pwm, pcfADC, PGType
PGType = PGFull
try:
pwm = PWM(0x40, debug = False)
pwm.setPWMFreq(60) # Set frequency to 60 Hz
except:
PGType = PGLite
GPIO.setmode(GPIO.BOARD)
GPIO.setup(irFR, GPIO.IN) #Right IR sensor module
GPIO.setup(irMID, GPIO.IN, pull_up_down=GPIO.PUD_DOWN) #Activation button
GPIO.setup(irFL, GPIO.IN, pull_up_down=GPIO.PUD_UP) #Left IR sensor module
speed = 40
#use pwm on inputs so motors don't go too fast
GPIO.setup(L1, GPIO.OUT)
p = GPIO.PWM(L1, 20)
p.start(0)
GPIO.setup(L2, GPIO.OUT)
q = GPIO.PWM(L2, 20)
q.start(0)
GPIO.setup(R1, GPIO.OUT)
a = GPIO.PWM(R1, 20)
a.start(0)
GPIO.setup(R2, GPIO.OUT)
b = GPIO.PWM(R2, 20)
b.start(0)
pcfADC = None # ADC object
try:
pcfADC = sgh_PCF8591P(1) #i2c, 0x48)
except:
PGType = PGLite
flag=0
while True:
j=GPIO.input(irMID)
if j==1: #Robot is activated when button is pressed
flag=1
print "Robot Activated",j
while flag==1:
i=GPIO.input(irFL) #Listening for output from right IR sensor
k=GPIO.input(irFR) #Listening for output from left IR sensor
if i==0: #Obstacle detected on right IR sensor
print "Obstacle detected on Right",i
#Move in reverse direction
p.ChangeDutyCycle(0)
q.ChangeDutyCycle(speed)
a.ChangeDutyCycle(0)
b.ChangeDutyCycle(speed)
q.ChangeFrequency(speed + 5)
b.ChangeFrequency(speed + 5)
#Turn robot left
p.ChangeDutyCycle(0)
q.ChangeDutyCycle(speed)
a.ChangeDutyCycle(speed)
b.ChangeDutyCycle(0)
q.ChangeFrequency(speed + 5)
a.ChangeFrequency(speed + 5)
if k==0: #Obstacle detected on left IR sensor
print "Obstacle detected on Left",k
p.ChangeDutyCycle(0)
q.ChangeDutyCycle(speed)
a.ChangeDutyCycle(0)
b.ChangeDutyCycle(speed)
q.ChangeFrequency(speed + 5)
b.ChangeFrequency(speed + 5)
p.ChangeDutyCycle(speed)
q.ChangeDutyCycle(0)
a.ChangeDutyCycle(0)
b.ChangeDutyCycle(speed)
p.ChangeFrequency(speed + 5)
b.ChangeFrequency(speed + 5)
elif i==0 and k==0:
print "Obstacles on both sides"
p.ChangeDutyCycle(0)
q.ChangeDutyCycle(speed)
a.ChangeDutyCycle(0)
b.ChangeDutyCycle(speed)
q.ChangeFrequency(speed + 5)
b.ChangeFrequency(speed + 5)
p.ChangeDutyCycle(speed)
q.ChangeDutyCycle(0)
a.ChangeDutyCycle(0)
b.ChangeDutyCycle(speed)
p.ChangeFrequency(speed + 5)
b.ChangeFrequency(speed + 5)
elif i==1 and k==1: #No obstacles, robot moves forward
print "No obstacles",i
#Robot moves forward
p.ChangeDutyCycle(speed)
q.ChangeDutyCycle(0)
a.ChangeDutyCycle(speed)
b.ChangeDutyCycle(0)
p.ChangeFrequency(speed + 5)
a.ChangeFrequency(speed + 5)
j=GPIO.input(irMID)
if j==1: #De activate robot on pushin the button
flag=0
print "Robot De-Activated",j
p.ChangeDutyCycle(0)
q.ChangeDutyCycle(0)
a.ChangeDutyCycle(0)
b.ChangeDutyCycle(0)
def init():
global p, q, a, b, pwm, pcfADC, PGType
global irFL, irFR, irMID, lineLeft, lineRight
GPIO.setwarnings(False)
PGType = PGFull
# Initialise the PCA9685 PWM device using the default address
try:
#pwm = PWM(0x40, debug = False)
#pwm.setPWMFreq(60) # Set frequency to 60 Hz
pca9685.init()
except:
PGType = PGLite # No PCA9685 so set to Pi2Go-Lite
# Initalise the ADC
pcfADC = None # ADC object
try:
pcfADC = sgh_PCF8591P(1) #i2c, 0x48)
except:
PGType = PGLite
#use physical pin numbering
GPIO.setmode(GPIO.BOARD)
# Define IR sensor pins - varies with model
if PGType == PGLite:
lineRight = lineRight_2
lineLeft = lineLeft_2
irFL = irFL_2
irFR = irFR_2
irMID = 0
else:
lineRight = lineRight_1
lineLeft = lineLeft_1
irFL = irFL_1
irFR = irFR_1
irMID = irMID_1
#set up digital line detectors as inputs
GPIO.setup(lineRight, GPIO.IN) # Right line sensor
GPIO.setup(lineLeft, GPIO.IN) # Left line sensor
#Set up IR obstacle sensors as inputs
GPIO.setup(irFL, GPIO.IN) # Left obstacle sensor
GPIO.setup(irFR, GPIO.IN) # Right obstacle sensor
if PGType == PGFull:
GPIO.setup(irMID, GPIO.IN) # Centre Front obstacle sensor
#use pwm on inputs so motors don't go too fast
GPIO.setup(L1, GPIO.OUT)
p = GPIO.PWM(L1, 20)
p.start(0)
GPIO.setup(L2, GPIO.OUT)
q = GPIO.PWM(L2, 20)
q.start(0)
GPIO.setup(R1, GPIO.OUT)
a = GPIO.PWM(R1, 20)
a.start(0)
GPIO.setup(R2, GPIO.OUT)
b = GPIO.PWM(R2, 20)
b.start(0)
# initialise servos (Pi2Go-Lite only)
if PGType == PGLite:
startServos()
#set up Pi2Go-Lite White LEDs as outputs
GPIO.setup(frontLED, GPIO.OUT)
GPIO.output(frontLED, 1) # switch front LEDs off as they come on by default
GPIO.setup(rearLED, GPIO.OUT)
GPIO.output(rearLED, 1) # switch rear LEDs off as they come on by default
#set switch as input with pullup
if PGType == PGLite:
GPIO.setup(Lswitch, GPIO.IN, pull_up_down=GPIO.PUD_UP)
else:
GPIO.setup(switch, GPIO.IN, pull_up_down=GPIO.PUD_UP)
# initialise wheel counters if Pi2Go-Lite
if PGType == PGLite:
threadC = threading.Thread(target = wheelCount)
threadC.start()
running = True
def init():
global p, q, a, b, pwm, pcfADC, PGType
PGType = PGFull
# Initialise the PCA9685 PWM device using the default address
try:
pwm = PWM(0x40, debug = False)
pwm.setPWMFreq(60) # Set frequency to 60 Hz
except:
PGType = PGLite # No PCA9685 so set to Pi2Go-Lite
#use physical pin numbering
GPIO.setmode(GPIO.BOARD)
#set up digital line detectors as inputs
GPIO.setup(lineRight, GPIO.IN) # Right line sensor
GPIO.setup(lineLeft, GPIO.IN) # Left line sensor
#Set up IR obstacle sensors as inputs
GPIO.setup(irFL, GPIO.IN) # Left obstacle sensor
GPIO.setup(irFR, GPIO.IN) # Right obstacle sensor
GPIO.setup(irMID, GPIO.IN) # Centre Front obstacle sensor
#use pwm on inputs so motors don't go too fast
GPIO.setup(L1, GPIO.OUT)
p = GPIO.PWM(L1, 20)
p.start(0)
GPIO.setup(L2, GPIO.OUT)
q = GPIO.PWM(L2, 20)
q.start(0)
GPIO.setup(R1, GPIO.OUT)
a = GPIO.PWM(R1, 20)
a.start(0)
GPIO.setup(R2, GPIO.OUT)
b = GPIO.PWM(R2, 20)
b.start(0)
# Initalise the ADC
pcfADC = None # ADC object
try:
pcfADC = sgh_PCF8591P(1) #i2c, 0x48)
except:
PGType = PGLite
# initialise servos (Pi2Go-Lite only)
if PGType == PGLite:
startServos()
#set up Pi2Go-Lite White LEDs as outputs
GPIO.setup(frontLED, GPIO.OUT)
GPIO.output(frontLED, 1) # switch front LEDs off as they come on by default
GPIO.setup(rearLED, GPIO.OUT)
GPIO.output(rearLED, 1) # switch rear LEDs off as they come on by default
#set switch as input with pullup
if PGType == PGLite:
GPIO.setup(Lswitch, GPIO.IN, pull_up_down=GPIO.PUD_UP)
else:
GPIO.setup(switch, GPIO.IN, pull_up_down=GPIO.PUD_UP)
# initialise wheel counters if Pi2Go-Lite
if PGType == PGLite:
threadC = threading.Thread(target = wheelCount)
threadC.start()
running = True
# Testing PCF8591 ADC Chip
import time
import sys
import RPi.GPIO as gpio
from sgh_PCF8591P import sgh_PCF8591P
pcfSensor = None
try:
pcfSensor = sgh_PCF8591P(0) #i2c, 0x48)
print pcfSensor
print "PCF8591P Detected"
except:
print "No PCF8591 Detected"
for i in range(5):
adc = pcfSensor.readADC(0) # get a value
print 'ADC1:', adc
time.sleep(1)
#!/usr/bin/python
# test8591.py
# read all four light sensors via the adc
# Author : Zachary Igielman
import time
import sys
import RPi.GPIO as gpio
from sgh_PCF8591P import sgh_PCF8591P
pcfSensor = None
try:
pcfSensor = sgh_PCF8591P(1) #i2c, 0x48)
print pcfSensor
print "PCF8591P Detected"
except:
print "No PCF8591 Detected"
while True:
adc1 = pcfSensor.readADC(0) # get a value
adc2 = pcfSensor.readADC(1) # get a value
adc3 = pcfSensor.readADC(2) # get a value
adc4 = pcfSensor.readADC(3) # get a value
print'ADC1:',adc1,' ADC2:',adc2,' ADC3:',adc3,' ADC4:',adc4
time.sleep(1)
# Set up IR obstacle sensors as inputs
GPIO.setup(7, GPIO.IN) # Left obstacle sensor
GPIO.setup(11, GPIO.IN) # Right obstacle sensor
GPIO.setup(15, GPIO.IN) # Centre Front obstacle sensor
# use pwm on inputs so motors don't go too fast
# Pins 24, 26 Left Motor
# Pins 19, 21 Right Motor
L1 = 26
L2 = 24
R1 = 19
R2 = 21
pcfADC = None
try:
pcfADC = sgh_PCF8591P(1) # i2c, 0x48)
print pcfADC
print "PCF8591P Detected"
except:
print "No PCF8591 Detected"
GPIO.setup(L1, GPIO.OUT)
p = GPIO.PWM(L1, 20)
p.start(0)
GPIO.setup(L2, GPIO.OUT)
q = GPIO.PWM(L2, 20)
q.start(0)
GPIO.setup(R1, GPIO.OUT)
a = GPIO.PWM(R1, 20)
def __init__(self, *args):
'''
Creates an instance of MyRobot and initalizes the GPIO.
'''
if MyRobot._myInstance != None:
raise Exception("Only one instance of MyRobot allowed")
global _isButtonEnabled
# 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.setup(SharedConstants.P_FRONT_CENTER, GPIO.IN)
GPIO.setup(SharedConstants.P_FRONT_RIGHT, GPIO.IN)
GPIO.setup(SharedConstants.P_LINE_LEFT, GPIO.IN)
GPIO.setup(SharedConstants.P_LINE_RIGHT, GPIO.IN)
# 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)
# I2C PWM chip for LEDs
Tools.debug("Trying to detect PCA9685 PCM chip on I2C bus using bus number 1...")
try:
self.ledPWM = PWM(0x40, busnumber = 1, debug = False)
self.ledPWM.setPWMFreq(SharedConstants.LED_PWM_FREQ)
Tools.debug("PCA9685 PCM chip on I2C bus detected")
except:
Tools.debug("Failed, trying with bus number 0...")
try:
self.ledPWM = PWM(0x40, busnumber = 0, debug = False)
self.ledPWM.setPWMFreq(SharedConstants.LED_PWM_FREQ)
Tools.debug("PCA9685 PCM chip on I2C bus detected")
except:
print "Failed to detect PCA9685 PCM chip on I2C bus"
sys.exit(1)
# I2C analog extender chip
Tools.debug("Trying to detect PCF8591 I2C analog extender on I2C bus 1...")
try:
self.analogExtender = sgh_PCF8591P(1) # i2c at address 0x48
Tools.debug("PCF8591 I2C analog extender on I2C bus # 1 detected")
except:
print "Failed, trying with bus number 0..."
try:
self.analogExtender = sgh_PCF8591P(0) # i2c at address 0x48
print "PCF8591 I2C analog extender on I2C bus # 0 detected"
except:
print "Failed to detect PCF8591 I2C analog extender on I2C bus"
sys.exit(1)
# if SharedConstants.BLINK_CONNECT_DISCONNECT:
# BlinkerThread(self, True)
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")
MyRobot._myInstance = self
