1) move the ECHO to GPIO26 (beyond-the-pi-droid-alpha-connector-on-PiBplus-andPi3B pin37)
2) allow TRIG to be connected to any of SERVO1 (pin0) to SERVO5 (pin4)
Therefore I propose the following mods to PDALib based on v0.93: (code at end)
- Added LibExit() as an overload for RoboPiExit() - I don't like mixed up PDALib.RoboPiExit()
Removed duplicate digitalWrite()
Moved the ECHO setup from pinMode(pin,ECHO) to setEcho(gpioPin), clearEcho(gpioPin)
Changed readDistance2(gpioNum,gpioNum) to (PDALib.pin,gpioNum) to map trigger to a SERVO Pin,
and Echo direct to gpio connector, and comment returns value in cm not mm
My working test program is HCSR04.py :
- Code: Select all
#!/usr/bin/python
#
# HC-SR04 interface test using PDALib
#
# Alan McDonley 10 June 2016
import PDALib
import time
import sys
import signal
EchoPin = 26 #GPIO26 is pin 37 of the PiB+ and Pi3B 40pin connector
TrigPin = 2 #PDALib "pin" = Servo3 connector (of 1-8) (GPIO18)
# ################## CONTROL-C HANDLER
# Callback and setup to catch control-C and quit program
def signal_handler(signal, frame):
print '\n** Control-C Detected'
PDALib.clearEcho(EchoPin)
PDALib.LibExit()
sys.exit(0)
# Setup the callback to catch control-C
signal.signal(signal.SIGINT, signal_handler)
# ##################
# my_echo1 = pi.callback(22, pigpio.RISING_EDGE, _echo1)
# my_echo0 = pi.callback(22, pigpio.FALLING_EDGE, _echo0)
# Use modified PDALib.v93.py pinMode(pin,ECHO) for GPIO22 ("pin 4" even though connection is direct to GPIO22)
# Use setEcho(gpioNUM) for GPIO27 (pin 37 on PiB+ and Pi3B connector)
# pinMode(EchoPin,ECHO)
PDALib.setEcho(EchoPin)
while 1:
# print(readDistance2(18,22)/58)
print( "Dist: %.2f cm" % PDALib.readDistance2(TrigPin,EchoPin))
time.sleep(1.0)
# my_echo1.cancel()
# my_echo0.cancel()
and the modified PDALib:
- Code: Select all
#!/usr/bin/python
#
# PDALib.py Python API for Pi Droid Alpha educational robot controller
#
# Copyright 2015 William Henning
# http://Mikronauts.com
#
# PDALib provides an implementation of the RoboPi advanced robot controller for
# the Pi Droid Alpha educational robot controller.
#
# Please see the Pi Droid Alpha User Manual for documentation
#
# Aug.25/2015: v0.90 release (readDistance() not reliable)
# Sep.21/2015: v0.91 fixed Pi GPIO INPUT/OUTPUT constants, byte endian issue
# Feb.26/2015 v0.92 added readDistance2(trig,echo) support for HC-SR04
# see http://www.mikronauts.com/raspberry-pi/gpio-experiments/raspberry-pi-and-hc-sr04-distance-sensor-interfacing-with-c-and-python/
# Apr.12/2016: v0.93 fixed spi open/close mismatches
# ALAN's MODS
# 10Jun2016 Added LibExit(), removed duplicate digitalWrite()
# 10Jun2016 Moved ECHO setup from pinMode(pin,ECHO) to setEcho(gpioPin), clearEcho(gpioPin)
# 10Jun2016 Changed readDistance2(gpioNum,gpioNum) to (PDALib.pin,gpioNum)
# to map trigger to a SERVO Pin, and Echo direct to gpio connector
# and comment returns value in cm not mm
import time
import spidev
import pigpio
# pin 0-7 = Pi GPIO's
# pin 8-23 = Dio bits
INPUT = 0
OUTPUT = 1
PWM = 2
SERVO = 3
PING = 4
TRIG = 5
ECHO = 6
mode = [0,0,0,0,0,0,0,0]
diomode = [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0]
servopin = [4, 17, 18, 27, 22, 23, 24, 25]
servoval = [0, 0, 0, 0, 0, 0, 0, 0]
req12 = [[6,0,0], [6,64,0],[6,128,0],[6,192,0],[7,0,0], [7,64,0],[7,128,0],[7,192,0]]
req10 = [[1,128,0], [1,144,0],[1,160,0],[1,176,0],[1,192,0], [1,208,0],[1,224,0],[1,240,0]]
DIO_DIR = 0x00 # I/O direction register, 0=output, 1=input
DIO_POL = 0x02 # polarity invert config, if 1 inverted logic for pin
DIO_GPPU = 0x0C # pullup configuration register, 1 enables pullup for pin
DIO_GPIO = 0x12 # pin values
DIO_OLAT = 0x14 # output latch
pi = pigpio.pi()
spi = spidev.SpiDev()
spi.open(0,0) # open(port,CS) port=0, CS=0|1
spi.max_speed_hz=1953000 # default speed undervalued divided voltages
spi.xfer([0x40,0xff,0xff]) # sets all pins as inputs
spi.close()
#
# Initialize library - for RoboPi compatibility, ignores arguments
# (not for Pi Droid Alpha) # Alan
def RoboPiInit(device, bps):
return 0
#
# Shut down library
#
def RoboPiExit():
pi.stop()
# Alan
def LibExit():
pi.stop()
#
# return the current mode of digital pins (0..23)
#
# 0..7 are Raspberry Pi pins on the Servo header (PWM, SERVO, INPUT, OUTPUT, PING)
# 8..23 are DIO A0-A7 and B0-B7 respectively (INPUT, OUTPUT)
#
def readMode(pin):
if pin < 0:
return -1
if pin < 8:
return mode[pin]
if pin < 24:
return diomode[pin-8]
return -1
#
# set the current mode of digital pins (0..23)
#
# 0..7 are Raspberry Pi pins on the Servo header (PWM, SERVO, INPUT, OUTPUT, PING)
# 8..23 are DIO A0-A7 and B0-B7 respectively (INPUT, OUTPUT)
#
def _echo1(gpio, level, tick):
global _high
_high = tick
def _echo0(gpio, level, tick):
global _done, _high, _time
_time = tick - _high
_done = True
# Alan moved from setMode(pin) to setEcho(gpio) and clearEcho(gpio)
#
# use setEcho(gpio) to initialize callbacks on the gpioNUM direct connection
# use clearEcho(gpio) to remove the callbacks
#
# Alan gpio is the gpioNUM - e.g. 22 for GPIO22 (pin15), or 27 for GPIO27 (pin 37)
def clearEcho(gpio):
global my_echo0, my_echo1
my_echo1.cancel()
my_echo0.cancel()
def setEcho(gpio):
global my_echo0, my_echo1
# my_echo1 = pi.callback(22, pigpio.RISING_EDGE, _echo1)
# my_echo0 = pi.callback(22, pigpio.FALLING_EDGE, _echo0)
# Alan - 10Jun2016 mod for generic ECHO direct to GPIO_Pin interface
my_echo1 = pi.callback(gpio, pigpio.RISING_EDGE, _echo1)
my_echo0 = pi.callback(gpio, pigpio.FALLING_EDGE, _echo0)
def pinMode(pin,newmode):
# Alan global my_echo0, my_echo1
if pin < 0:
return -1
if pin < 8: # only accept valid modes
if mode[pin] == PWM: # stop PWM if setting for input or output
pi.set_PWM_dutycycle(servopin[pin],0)
if mode[pin] == TRIG:
mode[pin] = newmode
if mode[pin] == ECHO:
mode[pin] = newmode
# Alan moved cancels to clearEcho
clearEcho(servopin[pin])
if newmode == INPUT:
mode[pin] = newmode
pi.set_mode(servopin[pin], pigpio.INPUT)
if newmode == OUTPUT:
mode[pin] = newmode
pi.set_mode(servopin[pin], pigpio.OUTPUT)
if newmode == PWM:
mode[pin] = newmode
pi.set_PWM_frequency(servopin[pin],490)
if newmode == PING:
mode[pin] = newmode
if newmode == TRIG:
mode[pin] = newmode
if newmode == ECHO:
setEcho(servopin[pin]) # Alan moved setup to setEcho
return 0
if pin < 24: # only accept valid modes
if newmode == INPUT:
diomode[pin-8] = newmode
setDioBit(0,pin-8)
return 0
if newmode == OUTPUT:
diomode[pin-8] = newmode
clearDioBit(0,pin-8)
return 0
return -1
#
# Read current value of digital pin (0..23)
#
# 0..7 are Raspberry Pi pins on the Servo header (PWM, SERVO, INPUT, OUTPUT, PING)
# 8..23 are DIO A0-A7 and B0-B7 respectively (INPUT, OUTPUT)
#
def digitalRead(pin):
if pin < 0:
return -1
if pin < 8:
return pi.read(servopin[pin])
if pin < 24:
return getDioBit(DIO_GPIO,pin-8)
return -1
#
# Set the value of digital pins (0..23)
#
# 0..7 are Raspberry Pi pins on the Servo header (PWM, SERVO, INPUT, OUTPUT, PING)
# 8..23 are DIO A0-A7 and B0-B7 respectively (INPUT, OUTPUT)
#
def digitalWrite(pin,val):
if pin < 0:
return -1
if pin < 8:
return pi.write(servopin[pin],val)
if pin < 24:
if val:
return setDioBit(DIO_OLAT,pin-8)
else:
return clearDioBit(DIO_OLAT,pin-8)
return -1
# A. McDonley removed duplicate digitalWrite()
#
# read SPI data from MCP3008 chip, 8 possible adc's (0 thru 7)
#
def analogRead(adcnum):
if adcnum < 0:
return -1
if adcnum > 7:
return -1
spi.open(0,1)
r = spi.xfer2(req10[adcnum])
spi.close()
adcout = ((r[1]&3) << 8) + r[2]
return adcout
#
# read SPI data from MCP3008 chip, 8 possible adc's (0 thru 7)
#
# shift left two bits to present 10 bit result as 12 bit result
# for RoboPi compatibility
#
def analogReadRaw(adcnum):
return analogRead(adcnum)<<2
#
# Set motor speed 0..255 for specified pin (mode must be PWM)
#
# default to 490Hz like Arduino
#
def analogWrite(pin,val):
if pin < 0:
return -1
if pin > 7:
return -1
pi.set_PWM_dutycycle(servopin[pin],val)
return 0
#
# Get servo position last written to specified pin
#
def servoRead(pin):
if pin < 0:
return -1
if pin > 7:
return -1
return servoval[pin]
#
# Set servo position for specified pin (mode must be SERVO)
#
def servoWrite(pin,val):
if pin < 0:
return -1
if pin > 7:
return -1
servoval[pin] = val
pi.set_servo_pulsewidth(servopin[pin], val)
return 0
#
# readDistance() is not working properly yet - look for it in future releases
#
def readDistance(pin):
if pin < 0:
return -1
if pin > 7:
return -1
if mode[pin] == PING:
pi.set_mode(servopin[pin], pi.OUTPUT)
pi.gpio_trigger(servopin[pin], 50, 1) # 50us, high pulse
pi.set_mode(servopin[pin], pi.INPUT)
ok = pi.wait_for_edge(servopin[pin], pigpio.RISING_EDGE, 0.01)
if ok:
t0 = pi.get_current_tick()
ok = pi.wait_for_edge(servopin[pin], pigpio.FALLING_EDGE, 0.1)
if ok:
t1 = pi.get_current_tick()
return (t1 - t0) # returns microseconds
return -1
#
# readDistance2() for HC-SR04 only
#
# for an explanation, see my article:
#
# http://www.mikronauts.com/raspberry-pi/gpio-experiments/raspberry-pi-and-hc-sr04-distance-sensor-interfacing-with-c-and-python/
#
# Alan _trig is a PDALib servo pin - suggest 0-4 (SERVO1-SERVO5) - no change
# Alan _echo is now a gpioNum - originally pin 4 gpio22. This change enables off-the-connector GPIO26 usage.
# Alan changed comment "returns value in cm" not mm
def readDistance2(_trig, _echo):
global pi, _done, _time
_done = False
pi.set_mode(servopin[_trig], pigpio.OUTPUT)
pi.gpio_trigger(servopin[_trig],50,1)
# Alan pi.set_mode(servopin[_echo], pigpio.INPUT)
pi.set_mode(_echo, pigpio.INPUT)
time.sleep(0.0001)
tim = 0
while not _done:
time.sleep(0.001)
tim = tim+1
if tim > 50:
return 0
return _time / 58.068 # 29.034 # return as CM #Alan
#
# The following functions are not meant for public use, and may not be supported
# in the future. If you use them, do not be surprised if future versions of the
# library do not make them available.
#
# The following functions are NOT available on RoboPi
#
#
# Read specified MCP23S17 register (16 bits)
#
def readDio(reg):
spi.open(0,0)
r = spi.xfer([0x41,reg,0,0])
spi.close()
return (r[3]<<8)+r[2]
#
# Write to specified MCP23S17 register (16 bits)
#
def writeDio(reg,val):
spi.open(0,0)
r = spi.xfer([0x40,reg,val&255,val>>8])
spi.close()
return r
#
# Set specific bit in specified register
#
def setDioBit(reg,bit):
if bit < 0:
return -1
if bit > 15:
return -1
t = readDio(reg)
t = t | (1<<bit)
writeDio(reg,t)
return 0
#
# Clear specific bit in specified register
#
def clearDioBit(reg,bit):
if bit < 0:
return -1
if bit > 15:
return -1
t = readDio(reg)
t = t & ~(1<<bit)
writeDio(reg,t)
return 0
#
# Flip value of specific bit in specified register
#
def flipDioBit(reg,bit):
if bit < 0:
return -1
if bit > 15:
return -1
t = readDio(reg)
t = t ^ (1<<bit)
writeDio(reg,t)
return 0
#
# Get value of specific bit in specified register
#
def getDioBit(reg,bit):
if bit < 0:
return -1
if bit > 15:
return -1
if readDio(reg) & (1<<bit):
return 1
return 0
#
# Print out contents of all the MCP23S17 registers (16 bit)
#
def dumpDio():
print "reg val"
print "--------"
for r in range(0,16):
print "", format(r+r, '02X'), format(readDio(r+r),'04X')