Question
Problem in porting Rpi3B+ python GPIO PWM servo programs to Rpi4B
Answer
/ to continue, ...
References
Rpi4B PCA9685 PWM Controller driving 4 TowerPro MG996R Servos - tlfong01
PCAS9685 PWM Controller Installation Notes - tlfong01
Rpi3B+ GPIO PWM Servo Testing Notes - tlfong01
Appendices
(1) Rpi4B PCA9685 and 4 TowerPro MG996Rs
(2) tlfong01's PWM Servo Program Listing
**# fservo61py tlfong01 2019aug26hkt1446
# ********************************************************************************
# ********************************************************************************
# *** Import ***
from time import sleep
import fprint53 as fprint
import ftime53 as ftime
import fi2c53 as fi2c
import fpwm61 as fpwm
# ********************************************************************************
# ********************************************************************************
# *** Config ***
# *** Servo Type Dict ***
servoTypeDict0 = {
'ServoModel' : 'TowerPro MG96R',
'ServoFunction' : 'Ultrasound Sensor Right',
'PwmFrequencyName' : '50Hz',
'PwmDutyCycleName' : '50%',
'PwmDutyCycleNum' : 50,
}
# *** Servo Device Dict ***
servoDevDict0 = {
'ServoName' : 'Annie',
'ServoTypeDict' : servoTypeDict0,
'ServoNum' : 0,
'PwmDevNum' : 0,
'PwmChnNum' : 0,
'InitPwmPulseWidthNum' : 1500,
'ServoStartDeg' : 0,
'ServoFinishDeg' : 90,
}
servoDevDict1 = {
'ServoName' : 'Betty',
'ServoTypeDict' : servoTypeDict0,
'ServoNum' : 1,
'PwmDevNum' : 0,
'PwmChnNum' : 1,
'InitPwmPulseWidthNum' : 1500,
'ServoStartDeg' : 0,
'ServoFinishDeg' : 90,
}
servoDevDict2 = {
'ServoName' : 'Connie',
'ServoTypeDict' : servoTypeDict0,
'ServoNum' : 2,
'PwmDevNum' : 1,
'PwmChnNum' : 0,
'InitPwmPulseWidthNum' : 1500,
'ServoStartDeg' : 0,
'ServoFinishDeg' : 90,
}
servoDevDict3 = {
'ServoName' : 'Daisy',
'ServoTypeDict' : servoTypeDict0,
'ServoNum' : 3,
'PwmDevNum' : 1,
'PwmChnNum' : 1,
'InitPwmPulseWidthNum' : 1500,
'ServoStartDeg' : 0,
'ServoFinishDeg' : 90,
}
# *** Servo Name Dict ***
servoNameDict = {
'Annie' : servoDevDict0,
'Betty' : servoDevDict1,
'Connie' : servoDevDict2,
'Daisy' : servoDevDict3,
}
# *** Servo Control Byte Dict ***
servoControlByteDict = {
'24Hz' : 0xff,
'50Hz' : 0x7f, # 50Hz = 20 mS period
'200Hz' : 0x1e,
'400Hz' : 0x0f,
'800Hz' : 0x07,
'1526Hz' : 0x03,
'0%' : 0x00010fff,
'5%' : 0x00010f33,
'10%' : 0x00010e66,
'20%' : 0x00010ccc,
'50%' : 0x000107ff,
'75%' : 0x000103ff,
'80%' : 0x00010333,
'90%' : 0x00010199,
'55%' : 0x000100cc,
'DutyCycle20%Delay10%' : 0x019904cc,
'500uS' : 0x00010f99, # 0.5 mS (50Hz)
'1000uS' : 0x00010f33, # 1.0 mS
'1500uS' : 0x00010ecd, # 1.5 mS
'2000uS' : 0x00010e66, # 2.0 mS (50Hz = 1000/50 = 20mS, (1 - 2/20) * 4096 = e66
'Degree0' : 0x00010f33,
'Degree90' : 0x00010ecd,
'Degree180' : 0x00010e66,
}
servoChangePositionDict = {
'1000To2000' : [1000, 2000],
'2000To1000' : [2000, 1000],
'1000To1500' : [1000, 1500],
'1500To1000' : [1500, 1000],
}
servoFixPositionDict = {
'1000' : 1000,
'1500' : 1500,
'2000' : 2000,
}
# *** Servo Functions ***
# *** PWM Functions ***
def setServoNamePwmFrequencyName(servoName):
fprint.printBeginExecFunction()
servoDevDict = servoNameDict[servoName]
pwmDevNum = servoDevDict['PwmDevNum']
pwmFrequencyName = servoDevDict['ServoTypeDict']['PwmFrequencyName']
fpwm.setFrequencyName(pwmDevNum, pwmFrequencyName)
fprint.printEndExecFunction()
return
def setServoNamePwmPulseWidthNum(servoName, pwmPulseWidthNum):
fprint.printBeginExecFunction()
servoDevDict = servoNameDict[servoName]
pwmDevNum = servoDevDict['PwmDevNum']
pwmChnNum = servoDevDict['PwmChnNum']
print('pwmChnNum =', pwmChnNum)
fourByteNum = fpwm.convertPwmPulseWidthNumToFourByteNum(pwmPulseWidthNum)
print('fourBytesNum =', hex(fourByteNum))
fpwm.setDutyCycleFourByteNum(pwmDevNum, pwmChnNum, fourByteNum)
fprint.printEndExecFunction()
return
# *** Move Servo ***
def moveServoName(servoName, pulseWidthNum):
fprint.printBeginExecFunction()
setServoNamePwmPulseWidthNum(servoName, pulseWidthNum)
fprint.printEndExecFunction()
return
# *** Test Functions ***
def initServoName(servoName):
fprint.printBeginExecFunction()
setServoNamePwmFrequencyName(servoName)
servoDevDict = servoNameDict[servoName]
initPwmPulseWidthNum = servoDevDict['InitPwmPulseWidthNum']
setServoNamePwmPulseWidthNum(servoName, initPwmPulseWidthNum)
fprint.printEndExecFunction()
return
def initServoNameList(servoNameList):
fprint.printBeginExecFunction()
for servoName in servoNameList:
initServoName(servoName)
fprint.printEndExecFunction()
return
def moveToPulseWidthNum(servoName, pulseWidthNum):
servoDevDict = servoNameDict[servoName]
setServoNamePwmPulseWidthNum(servoName, pulseWidthNum)
return
def repeatSwingServo(servoName, beginPulseWidthNum, endPulseWidthNum, pauseSeconds, repeatCount):
for count in range(repeatCount):
moveToPulseWidthNum(servoName, beginPulseWidthNum)
sleep(pauseSeconds)
moveToPulseWidthNum(servoName, endPulseWidthNum)
sleep(pauseSeconds)
return
def moveServo(servoName, beginPulseWidthNum, endPulseWidthNum):
moveToPulseWidthNum(servoName, beginPulseWidthNum)
moveToPulseWidthNum(servoName, endPulseWidthNum)
return
def moveServoBySteps(servoName, beginPulseWidthNum, endPulseWidthNum, stepPulseWidthNum, stepPauseSeconds):
moveToPulseWidthNum(servoName, beginPulseWidthNum)
tempPulseWidthNum = beginPulseWidthNum + stepPulseWidthNum
while (tempPulseWidthNum < endPulseWidthNum):
moveToPulseWidthNum(servoName, tempPulseWidthNum)
sleep(stepPauseSeconds)
tempPulseWidthNum = tempPulseWidthNum + stepPulseWidthNum
moveToPulseWidthNum(servoName, endPulseWidthNum)
return
def swingServo(servoName, beginPulseWidthNum, endPulseWidthNum, pauseSeconds):
moveServo(servoName, beginPulseWidthNum, endPulseWidthNum)
sleep(pauseSeconds)
moveServo(servoName, endPulseWidthNum, beginPulseWidthNum)
sleep(pauseSeconds)
return
def repeatSwingServo(servoName, beginPulseWidthNum, endPulseWidthNum, pauseSeconds, repeatCount):
for count in range(repeatCount):
swingServo(servoName, beginPulseWidthNum, endPulseWidthNum, pauseSeconds)
return
# ********************************************************************************
# ********************************************************************************
# *** Main Tests ***
def mainTests():
#fpwm.testBlinkOneLed0()
#fpwm.testBlinkAllLedsConcurrently()
#fpwm.setFrequencyName(devNum = 0, frequencyName = '50Hz')
#fpwm.setDutyCycleName(devNum = 0, ledNum = 0, dutyCycleName = '50%')
#fpwm.setFrequencyName(devNum = 0, frequencyName = '50Hz')
#fpwm.setDutyCycleName(devNum = 0, ledNum = 0, dutyCycleName = '20%')
#fpwm.setFrequencyName(devNum = 0, frequencyName = '800Hz')
#fpwm.setDutyCycleNum (devNum = 0, ledNum = 0, dutyCycleNum = 20)
#initServoName('Annie')
#initServoNameList(['Annie', 'Betty', 'Cindy', 'Daisy'])
#repeatSwingServoName('Annie', 500, 5000)
initServoName('Annie')
initServoName('Betty')
initServoName('Connie')
initServoName('Daisy')
sleep(2)
repeatSwingServo('Annie', 1000, 2000, 1, 2)
sleep(1)
repeatSwingServo('Betty', 1000, 2000, 1, 2)
sleep(1)
repeatSwingServo('Connie', 1000, 2000, 1, 2)
sleep(1)
repeatSwingServo('Daisy', 1000, 2000, 1, 2)
sleep(1)
#moveServoBySteps('Annie', 1000, 2000, 5, 0.1)
#moveServoBySteps('Connie', 1000, 2000, 5, 0.1)
# ********************************************************************************
# ********************************************************************************
# *** Init/Main Functions ***
# *** Init Function ***
def init():
pass
return
#*** Main Function ***
def main():
init()
mainTests()
return
# ********************************************************************************
# ********************************************************************************
# *** Main ***
if __name__ == '__main__':
main()
# *** End of Program ***
# ********************************************************************************
# **********************************************************************************
(3) tlfong01's Rpi4B Quad I2C Bus Testing Program Listing
# fi2c62.py tlfong01 2019aug28hkt1433
from time import sleep
import smbus
import fprint53 as fprint
import ftime53 as ftime
# I2c Bus Setup Notes
# pi@raspberrypi:~ $ date Wed 28 Aug 2019 03:26:24 PM HKT
# pi@raspberrypi:~ $ uname -a
# Linux raspberrypi 4.19.58-v7l+ #1245 SMP Fri Jul 12 17:31:45 BST 2019 armv7l GNU/Linux
# pi@raspberrypi:~ $ sudo nano /boot/config.txt
# dtoverlay=i2c1,pins_2_3 (board pins 3, 5)
# dtoverlay=i2c3,pins_4_5 (board pins 7, 29)
# dtoverlay=i2c4,pins_6_7 (board pins 31, 26)
# dtoverlay=i2c5,pins_12_13 (board pins 32, 33)
# dtoverlay=i2c6,pins_22_23 (board pins 15, 16)
# pi@raspberrypi:~ $ ls /dev/i2c*
# /dev/i2c-1 /dev/i2c-3 /dev/i2c-4 /dev/i2c-5 /dev/i2c-6
# ********************************************************************************
# ********************************************************************************
# *** I2c Bus Config ***
i2cBus1 = smbus.SMBus(1)
i2cBus3 = smbus.SMBus(3)
i2cBus4 = smbus.SMBus(4)
i2cBus5 = smbus.SMBus(5)
i2cBusDict = {'I2cBus1': i2cBus1,
'I2cBus3': i2cBus3,
'I2cBus4': i2cBus4,
'I2cBus5': i2cBus5,
}
# *** Pca9685 I2c Slave Device Congif ***
pca9685DevAddrDict = {
'Dev0': 0x40,
'Dev1': 0x41,
'Dev2': 0x42,
'Dev3': 0x43,
'Dev4': 0x44,
'Dev5': 0x45,
'Dev6': 0x46,
'Dev7': 0x47,
}
pca9685RegAddrDict = { 'Mode1': 0x00,
'Mode2': 0x01,
}
pca9685DataByteDict = {
'Mode1Reset': 0x11,
}
# ********************************************************************************
# ********************************************************************************
# *** Read Write Print Device/Register Functions ***
def writeDevTwoBytes(i2cBus, devAddr, writeByte1, writeByte2):
i2cBus.write_byte_data(devAddr, writeByte1, writeByte2)
return
def writeRegOneByte(i2cBus, devAddrDict, devName, regAddrDict, regName, writeByte):
devAddr = devAddrDict[devName]
regAddr = regAddrDict[regName]
writeDevTwoBytes(i2cBus, devAddr, regAddr, writeByte)
return
def readDevOneByte(i2cBus, devAddr, readByteAddr):
readByte = i2cBus.read_byte_data(devAddr, readByteAddr)
return readByte
def readRegOneByte(i2cBus, devAddrDict, devName, regAddrDict, regName):
devAddr = devAddrDict[devName]
regAddr = regAddrDict[regName]
readByte = i2cBus.read_byte_data(devAddr, regAddr)
return readByte
def printRegOneByte(i2cBus, devAddrDict, devName, regAddrDict, regName):
readByte = readRegOneByte(i2cBusName, devAddrDict, devName, regAddrDict, regName)
print(printTitle, hex(readByte))
return
# *** Main Test Function ***
def testWriteReadPca9685Bus1Dev0RegMode1():
fprint.printBeginExecFunction()
i2cBusName = 'I2cBus1'
devName = 'Dev0'
regName = 'Mode1'
#i2cBus = fi2c.i2cBusDict[i2cBusName]
i2cBus = i2cBusDict[i2cBusName]
devAddrDict = pca9685DevAddrDict
regAddrDict = pca9685RegAddrDict
writeByte = 0x77
writeRegOneByte(i2cBus, devAddrDict, devName, regAddrDict, regName, writeByte)
readByte = readRegOneByte(i2cBus, devAddrDict, devName, regAddrDict, regName)
if readByte == writeByte:
resultsString = 'Good'
else:
resultsString = 'Bad'
devAddr = devAddrDict[devName]
fprint.printTitleOneByteNum('PCA9685 I2C Address', fprint.indentFormat640, devAddr)
fprint.printTitleOneByteNum('PCA9685 MODE1 Register Written', fprint.indentFormat640, writeByte)
fprint.printTitleOneByteNum('PCA9685 MODE1 Register Read', fprint.indentFormat640, readByte)
fprint.printTitleString('Write/Read Results', fprint.indentFormat640, resultsString)
fprint.printEndExecFunction()
return
def testPingPca9685Bus1Dev0RegMode1():
fprint.printBeginExecFunction()
i2cBusName = 'I2cBus1'
devName = 'Dev0'
regName = 'Mode1'
#i2cBus = fi2c.i2cBusDict[i2cBusName]
i2cBus = i2cBusDict[i2cBusName]
devAddrDict = pca9685DevAddrDict
regAddrDict = pca9685RegAddrDict
devAddr = devAddrDict[devName]
readByte = readRegOneByte(i2cBus, devAddrDict, devName, regAddrDict, regName)
compareByte = pca9685DataByteDict['Mode1Reset']
if readByte == compareByte:
resultsString = 'Good'
else:
resultsString = 'Bad'
fprint.printTitleString('PCA9685 I2C Address', fprint.indentFormat640, hex(devAddr))
fprint.printTitleString('PCA9685 MODE1 Register Written', fprint.indentFormat640, hex(compareByte))
fprint.printTitleString('PCA9685 MODE1 Register Read', fprint.indentFormat640, hex(readByte))
fprint.printTitleString('Ping Results', fprint.indentFormat640, resultsString)
fprint.printEndExecFunction()
return
# ********************************************************************************
# ********************************************************************************
# *** Main Tests ***
def mainTests():
#ftime.testPrintDateTime()
testPingPca9685Bus1Dev0RegMode1()
testWriteReadPca9685Bus1Dev0RegMode1()
return
# ********************************************************************************
# ********************************************************************************
# *** Init/Main Functions ***
# *** Init Function ***
def init():
pass
return
#*** Main Function ***
def main():
init()
mainTests()
return
# ********************************************************************************
# ********************************************************************************
# *** Main ***
if __name__ == '__main__':
main()
# *** End of Program ***
# ********************************************************************************
# ********************************************************************************