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 ***
# ********************************************************************************
# **********************************************************************************