Uncategorized

pico 4WD code

*** pico_4wd_v94.py – tlfong01, 2021nov19hkt2223 ***

from machine import Pin, PWM, UART
import utime

========= ========= ========= ========= ========= ========= ========= ========= ========= =========

========= ========= ========= ========= ========= ========= ========= ========= ========= =========

========= ========= ========= ========= ========= ========= ========= ========= ========= =========

========= ========= ========= ========= ========= ========= ========= ========= ========= =========

*** UART GP pin assignment and setup/configuration ***

uart0 = UART(0, baudrate = 9600, tx=Pin(0), rx=Pin(1))
uart1 = UART(1, baudrate = 9600, tx=Pin(8), rx=Pin(9))

uartPortDict = \
{
‘0’: uart0,
‘1’: uart1,
}

HC12 GP pin assignment and setup/configuration ***

hc12SetupPinNum0 = 2
hc12SetupPinNum1 = 3

hc12SetupPin0 = Pin(hc12SetupPinNum0, Pin.OUT)
hc12SetupPin1 = Pin(hc12SetupPinNum1, Pin.OUT)

hc12SetupPinDict = \
{
‘0’: hc12SetupPin0,
‘1’: hc12SetupPin1,
}

hc12ConfigParameterDict = \
{
‘CheckAtReply’ : ‘AT’,
‘CheckAllConfigSetting’ : ‘AT+RX’,
‘SetChannel#1’ : ‘AT+C001’,
‘SetChannel#2’ : ‘AT+C002’,
}

hc12TextMsgDict = \
{
‘HelloWorld’ : ‘Hello World’,
‘HelloPico0’ : ‘Hello Pico #0’,
‘HelloPico1’ : ‘Hello Pico #1’,
‘MoveForward’ : ‘Move Forward’,
‘MoveBackward’ : ‘Move Backward’,
‘Stop’ : ‘Stop’
}

*** Uart Functions ***

def uartWrite(uartPortNum, writeBytes):
uartPort = uartPortDict[str(uartPortNum)]
uartPort.write(writeBytes)
return

def uartRead(uartPortNum):
uartPort = uartPortDict[str(uartPortNum)]
uartAny = uartPort.any()
print(‘ uartAny =’, uartAny)
readBytes = uartPort.read()
return readBytes

*** test uart loopback functions ***

def testUartLoopBack(uartPortNum, writeBytes):
print(‘ Begin testUartLoopBack(), …’)
print(‘ uartPortNum =’, uartPortNum)

uartWrite(uartPortNum, writeBytes)
print('        writeBytes       =', writeBytes)    

utime.sleep(0.5)

readBytes = uartRead(uartPortNum)     
print('        readBytes        =', readBytes)

print('    End   testUartLoopBack().')
return

*** UARTTest Functions ***

testUartLoopBack(uartPortNum = 0, writeBytes = ‘uart0 Hello World’)

testUartLoopBack(uartPortNum = 1, writeBytes = ‘uart1 Hello World’)

*** Sample output ***

”’

%Run -c $EDITOR_CONTENT
Begin testUartLoopBack(), …
uartPortNum = 0
writeBytes = uart0 Hello World
uartAny = 17
readBytes = b’uart0 Hello World’
End testUartLoopBack().
Begin testUartLoopBack(), …
uartPortNum = 1
writeBytes = uart1 Hello World
uartAny = 17
readBytes = b’uart1 Hello World’
End testUartLoopBack().

”’

========= ========= ========= ========= ========= ========= ========= =========

========= ========= ========= ========= ========= ========= ========= =========

hc12UartPortNumDict = \
{
‘0’: 0,
‘1’: 1,
}

*** HC12 Functions ***

def setHc12ConfigMode(hc12Num):
uartPortNum = hc12UartPortNumDict[str(hc12Num)]
hc12SetupPin = hc12SetupPinDict[str(uartPortNum)]
hc12SetupPin.low()
return

def setHc12CommMode(hc12Num):
uartPortNum = hc12UartPortNumDict[str(hc12Num)]
hc12SetupPin = hc12SetupPinDict[str(uartPortNum)]
hc12SetupPin.high()
return

*** HC12 Test Functions ***

def testHc12Config(hc12Num, writeBytes):
print(‘Begin testHc12Config(), …’)
uartPortNum = hc12UartPortNumDict[str(hc12Num)]
setHc12ConfigMode(uartPortNum)
utime.sleep(0.1)
testUartLoopBack(uartPortNum, writeBytes)
print(‘End testHc12Config(), …’)
return

*** Sample Tests ***

testHc12Config(hc12Num = 0, writeBytes = ‘AT’)

testHc12Config(hc12Num = 1, writeBytes = ‘AT’)

”’ *** Sample output ***

%Run -c $EDITOR_CONTENT
Begin testHc12Config(), …
Begin testUartLoopBack(), …
uartPortNum = 0
writeBytes = AT
uartAny = 4
readBytes = b’OK\r\n’
End testUartLoopBack().
End testHc12Config(), …
Begin testHc12Config(), …
Begin testUartLoopBack(), …
uartPortNum = 1
writeBytes = AT
uartAny = 4
readBytes = b’OK\r\n’
End testUartLoopBack().
End testHc12Config(), …

”’

def testHc12Communication(xmitUartPortNum, recvUartPortNum, testXmitMsg):
print(‘Begin testHc12Communication(), …’)

# *** Set both HC12 communication mode, ...')
print('  Begin set both HC12s communication mode, ...')
setHc12CommMode(xmitUartPortNum)
setHc12CommMode(recvUartPortNum)
print('  End   set both HC12s communication mode.')

print('  Begin transmit message, ...')
print('    xmitBytes =', testXmitMsg)
uartWrite(xmitUartPortNum, testXmitMsg)    
print('  End transmit message.')        

# *** uart1 recv test message ***
print('  Begin receive message, ...')
utime.sleep(0.1)    
recvBytes = uartRead(recvUartPortNum)
print('    recvBytes =', recvBytes)
print('  End   receive msaages.')
print('End   testHc12Communication(), ...')     
return

*** Sample Tests ***

testHc12Communication(xmitUartPortNum = 0, recvUartPortNum = 1, \
testXmitMsg = ‘Pico #1 at COM 11, uart0 transmitting, …’)

”’ *** Sample Output ***

%Run -c $EDITOR_CONTENT
Begin testHc12Communication(), …
Begin set both HC12s communication mode, …
End set both HC12s communication mode.
Begin transmit message, …
xmitBytes = Pico #1 at COM 11, uart0 transmitting, …
End transmit message.
Begin receive message, …
uartAny = 42
recvBytes = b’Pico #1 at COM 11, uart0 transmitting, …’
End receive msaages.
End testHc12Communication(), …

”’

========= ========= ========= ========= ========= ========= ========= =========

========= ========= ========= ========= ========= ========= ========= =========

*** Pico GP pin assignments ***

uart0TxDNum = 0 # HC12 #1 TxD
uart0RdDNum = 1 # HC12 #2 RxD

hc12Setup1 = 2 # Reserved for SPI0 Sck
hc12Setup2 = 3 # Reserved for SPI0 Tx
i2c0Sda = 4 # Reserved for SPI0 Rx
i2c0Scl = 5 # Reserved for SPI0 Csn

*** PWM Pins ***

aPwmPinNum0 = 6 #0
bPwmPinNum0 = 7 #1

uart1TxdNum = 8 # HC12 #2 TxD
uart1RxdNum = 9 # HC12 #2 RxD

aPwmPinNum1 = 11 #2
bPwmPinNum1 = 10 #3

*** Inrerrupt Pins ***

intPinNum0 = 12 #4
intPinNum1 = 13 #5
intPinNum2 = 14 #6
intPinNum3 = 15 #7

*** Motor Driver Control Pins (TB6612FNG) ***

stdByPinNum0 = 16 #8
stdByPinNum1 = 17 #9

aIn1PinNum0 = 18 #10
aIn2PinNum0 = 19 #11
bIn1PinNum0 = 20 #12
bIn2PinNum0 = 21 #13

aIn1PinNum1 = 27 #14
aIn2PinNum1 = 28 #15
bIn1PinNum1 = 22 #16
bIn2PinNum1 = 26 #17

*** Interrupt Pin Dicts ***

intGpPinNumDict = {‘0’: intPinNum0,
‘1’: intPinNum1,
‘2’: intPinNum2,
‘3’: intPinNum3,
}

intPinNumList = [intPinNum0, intPinNum1, intPinNum2, intPinNum3]

========= ========= ========= ========= ========= ========= ========= ========= ========= =========

========= ========= ========= ========= ========= ========= ========= ========= ========= =========

*** Sleep time dicts ***

secondsDict = {‘OneSecond’ : 1,
‘TwoSeconds’ : 2,
‘FourSeconds’ : 4,
‘TenSeconds’ : 10,
‘SixtySeconds’ : 60,
‘ThreeMinutes’ : 180,
}

def hold(secondsName):
utime.sleep(secondsDict[secondsName])
return

========= ========= ========= ========= ========= ========= ========= ========= ========= =========

========= ========= ========= ========= ========= ========= ========= ========= ========= =========

*** Interrupt global variables and dicts ***

global intCount0
global intCount1
global intCount2
global intCount3

intCount0 = 0
intCount1 = 0
intCount2 = 0
intCount3 = 0

intCountDict = {
‘0’: intCount0,
‘1’: intCount1,
‘2’: intCount2,
‘3’: intCount3,
}

*** Interrupt Callback Functions ***

def intCallBack0(pin):
global intCount0
intCount0 = intCount0 + 1
return

def intCallBack1(pin):
global intCount1
intCount1 = intCount1 + 1
return

def intCallBack2(pin):
global intCount2
intCount2 = intCount2 + 1
return

def intCallBack3(pin):
global intCount3
intCount3 = intCount3 + 1
return

intCallBackDict = {
‘0’: intCallBack0,
‘1’: intCallBack1,
‘2’: intCallBack2,
‘3’: intCallBack3,
}

def countIntPinIntPeriod(intPinNum, countPeriod):
global intCount0
global intCount1
global intCount2
global intCount3

intCount0 = 0
intCount1 = 0
intCount2 = 0
intCount3 = 0

utime.sleep(countPeriod)

if intPinNum  == 0:
    intCount = intCount0
elif intPinNum == 1:    
    intCount = intCount1
elif intPinNum == 2:    
    intCount = intCount2
else:    
    intCount = intCount3                
return intCount

def countIntPinNumListIntPeriod(intPinNumList, countPeriod):
intCountList = [0] * len(intPinNumList)
for index in range(len(intPinNumList)):
intCountList[index] = countIntPinIntPeriod(intPinNumList[index], countPeriod)
return intCountList

*** Test functions ***

def repeatCountIntPinNumListIntPeriod(intPinNumList, countPeriod, pauseTime, repeatTimes):
print(‘\n countIntPinNumListIntPeriod()’)
intGpPinNumList = [0] * len(intPinNumList)
for index in range(len(intGpPinNumList)):
intGpPinNumList[index] = intGpPinNumDict[str(index)]
print(‘ intPinNumList =’, intPinNumList)
print(‘ intGpPinNumList =’, intGpPinNumList)
print(‘ countPeriod (seconds) =’, countPeriod)
print(‘ pauseTime (seconds) =’, pauseTime)
print(‘ repeat count times =’, repeatTimes)
print(”)

for count in range(repeatTimes):
    ppsList             = countIntPinNumListIntPeriod(intPinNumList, countPeriod)
    print('    countList =', ppsList, end = '')
    print(' , min ', min(ppsList), end = '')
    print(' , max ', max(ppsList), end = '')
    print(' , dif ', max(ppsList) - min(ppsList), end = '')
    print(' , avg ', int(sum(ppsList) / len(ppsList)))      
    utime.sleep(pauseTime)    
return

========= ========= ========= ========= ========= ========= ========= ========= ========= =========

========= ========= ========= ========= ========= ========= ========= ========= ========= =========

defaultDirection = ‘Forward’
defaultSpeed = ‘VertHighSpeed’
defaultPwmFreq = 1000
defaultDutyCycle = 90

dutyCycleDict = {
‘VeryVerySlow’ : 95,
‘VeryFast’ : 90,
‘Fast’ : 80,
‘Normal’ : 50,
‘Slow’ : 40,
‘VerySlow’ : 30,
‘VeryVerySlow’ : 25,
}

motorConfigDictDict = \
{
‘0’: {‘MotorDriverNum’: 0, ‘ChannelNum’: 0},
‘1’: {‘MotorDriverNum’: 0, ‘ChannelNum’: 1},
‘2’: {‘MotorDriverNum’: 1, ‘ChannelNum’: 0},
‘3’: {‘MotorDriverNum’: 1, ‘ChannelNum’: 1},
}

motorDriverGpPinNumDict = { \
‘0’: {‘StdByPinNum’ : stdByPinNum0,
‘0’ : {‘In1PinNum’ : aIn1PinNum0,
‘In2PinNum’ : aIn2PinNum0,
‘PwmPinNum’ : aPwmPinNum0,
‘PwmFreq’ : defaultPwmFreq,
‘DutyCycle’ : defaultDutyCycle,
‘IntPinNum’ : intPinNum0,
‘IntPinCallBack’ : intCallBack0,
},
‘1’ : {‘In1PinNum’ : bIn1PinNum0,
‘In2PinNum’ : bIn2PinNum0,
‘PwmPinNum’ : bPwmPinNum0,
‘PwmFreq’ : defaultPwmFreq,
‘DutyCycle’ : defaultDutyCycle,
‘IntPinNum’ : intPinNum1,
‘IntPinCallBack’ : intCallBack1,
},
},
‘1’: {‘StdByPinNum’ : stdByPinNum1,
‘0’ : {‘In1PinNum’ : aIn1PinNum1,
‘In2PinNum’ : aIn2PinNum1,
‘PwmPinNum’ : aPwmPinNum1,
‘PwmFreq’ : defaultPwmFreq,
‘DutyCycle’ : defaultDutyCycle,
‘IntPinNum’ : intPinNum2,
‘IntPinCallBack’ : intCallBack2,
},
‘1’ : {‘In1PinNum’ : bIn1PinNum1,
‘In2PinNum’ : bIn2PinNum1,
‘PwmPinNum’ : bPwmPinNum1,
‘PwmFreq’ : defaultPwmFreq,
‘DutyCycle’ : defaultDutyCycle,
‘IntPinNum’ : intPinNum3,
‘IntPinCallBack’ : intCallBack3
},
},
}

def setupMotor(motorNum):
motorConfigDict = motorConfigDictDict[str(motorNum)]
motorDriverNum = motorConfigDict[‘MotorDriverNum’]
channelNum = motorConfigDict[‘ChannelNum’]

stdByPinNum     = motorDriverGpPinNumDict[str(motorDriverNum)]['StdByPinNum']
in1PinNum       = motorDriverGpPinNumDict[str(motorDriverNum)][str(channelNum)]['In1PinNum']
in2PinNum       = motorDriverGpPinNumDict[str(motorDriverNum)][str(channelNum)]['In2PinNum']

pwmPinNum       = motorDriverGpPinNumDict[str(motorDriverNum)][str(channelNum)]['PwmPinNum']
pwmFreq         = motorDriverGpPinNumDict[str(motorDriverNum)][str(channelNum)]['PwmFreq']    
dutyCycle       = motorDriverGpPinNumDict[str(motorDriverNum)][str(channelNum)]['DutyCycle']     

intPinNum       = motorDriverGpPinNumDict[str(motorDriverNum)][str(channelNum)]['IntPinNum']
intPinCallBack  = motorDriverGpPinNumDict[str(motorDriverNum)][str(channelNum)]['IntPinCallBack']

# *** Motor Driver Control Pin Setup ***    

stdByPin    = Pin(stdByPinNum, Pin.OUT)
in1Pin      = Pin(in1PinNum, Pin.OUT)    
in2Pin      = Pin(in2PinNum, Pin.OUT)
intPin      = Pin(intPinNum, Pin.IN, Pin.PULL_DOWN)

stdByPin.high()
in1Pin.low()
in2Pin.low()

# *** PWM Pin Setup ***      

pwmPin      = PWM(Pin(pwmPinNum))
pwmPin.freq(pwmFreq)
u16DutyCycle = int((dutyCycle / 100) * 65536)  
pwmPin.duty_u16(u16DutyCycle)

# *** Interrupt Pin Setup ***

intPin.irq(intPinCallBack, Pin.IRQ_FALLING)

# *** Motor Control Dict ***

motorControlDict = {'StdByPin': stdByPin,
                    'In1Pin'  : in1Pin,
                    'In2Pin'  : in2Pin,
                    'PwmPin'  : pwmPin,
                    'IntPin'  : intPin,
                   }

motorStatusDict = {'Direction'   : defaultDirection,
                   'Speed'       : defaultSpeed,
                   'PwmFrequency': defaultPwmFreq,
                   'DutyCycle'   : defaultDutyCycle,
                  }                       

motorDict = {'MotorConfictDict' : motorConfigDict,
             'MotorControlDict' : motorControlDict,
             'MotorStatusDict'  : motorStatusDict
            }    

return motorDict

def setupMotorList(motorNumList):
motorControlDictList = [0] * len(motorNumList)
for motorNum in motorNumList:
motorControlDict = setupMotor(motorNum)
motorControlDictList[motorNum] = motorControlDict
return motorControlDictList

def setupMotorDirection(motorControlDict, directionName):
if directionName ==’Forward’:
motorControlDict[‘In1Pin’].low()
motorControlDict[‘In2Pin’].high()
elif directionName == ‘Backward’:
motorControlDict[‘In1Pin’].high()
motorControlDict[‘In2Pin’].low()
return

def setupMotorSpeed(motorControlDict, speedName):
pwmPin = motorControlDict[‘PwmPin’]
dutyCycle = dutyCycleDict[speedName]
pwmPin.duty_u16(int((dutyCycle / 100) * 65536) )
return

def moveMotorDirectionSpeed(motorControlDict, directionName, speedName):
moveMotorDirection(motorControlDict, directionName)
moveMotorSpeed(motorControlDict, speedName)
return

def moveMotorDirectionSpeedList(motorControlDictList, directionName, speedName):
for motorControlDict in motorControlDictList:
moveMotorDirectionSpeed(motorControlDict, directionName, speedName)
return

def stopMotor(motorControlDict):
motorControlDict[‘In1Pin’].low()
motorControlDict[‘In2Pin’].low()
return

def moveMotorForward(motorControlDict):
motorControlDict[‘In1Pin’].high()
motorControlDict[‘In2Pin’].low()
return

def stopMotorList(motorControlDictList):
for motorControlDict in motorControlDictList:
stopMotor(motorControlDict)
return

*** Test setup/stop/move motor functions ***

def testSetupMotor(motorNum):
print(‘Begin testSetupMotor(), ‘, ‘motorNum =’, motorNum)
motorControlDict = setupMotor(motorNum)[‘MotorControlDict’]
stopMotor(motorControlDict)
print(‘End testSetupMotor().’)
return

def testSetupMotorList(motorNumList):
for motorNum in motorNumList:
testSetupMotor(motorNum)
return

def testSetupMoveMotor(motorNum, directionName, speedName, holdSecondsName):
print(‘Begiun testSetupMoveMotor(), ‘, ‘motorNum =’, motorNum)
motorDict = setupMotor(motorNum)
motorControlDict = motorDict[‘MotorControlDict’]
stopMotor(motorControlDict)
setupMotorDirection(motorControlDict, directionName)
setupMotorSpeed(motorControlDict, speedName)
hold(holdSecondsName)
stopMotor(motorControlDict)
print(‘End testSetupMoveMotor().’)
return

def testSetupMoveMotorList(motorNumList, directionName, speedName, holdSecondsName):
print(‘Begiun testSetupMoveMotorNumList(), …’)
for motorNum in motorNumList:
motorDict = setupMotor(motorNum)
motorControlDict = motorDict[‘MotorControlDict’]
stopMotor(motorControlDict)
setupMotorDirection(motorControlDict, directionName)
setupMotorSpeed(motorControlDict, speedName)
hold(holdSecondsName)
stopMotor(motorControlDict)
print(‘End testSetupMoveMotorNumList().’)
return

========= ========= ========= ========= ========= ========= ========= =========

========= ========= ========= ========= ========= ========= ========= =========

*** Main ***

testSetupMotor(motorNum = 0)

testSetupMotorList(motorNumList = [0, 1, 2, 3])

testSetupMoveMotor(motorNum = 0, directionName = ‘Forward’, speedName = ‘VeryFast’, holdSecondsName = ‘TwoSeconds’)

testSetupMoveMotor(motorNum = 1, directionName = ‘Forward’, speedName = ‘VeryFast’, holdSecondsName = ‘TwoSeconds’)

testSetupMoveMotor(motorNum = 2, directionName = ‘Forward’, speedName = ‘VeryFast’, holdSecondsName = ‘TwoSeconds’)

testSetupMoveMotor(motorNum = 3, directionName = ‘Forward’, speedName = ‘VeryFast’, holdSecondsName = ‘TwoSeconds’)

testSetupMoveMotorList([0, 1, 2, 3], ‘Forward’, ‘VeryFast’, ‘TwoSeconds’)

*** End ***

Categories: Uncategorized

Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out /  Change )

Google photo

You are commenting using your Google account. Log Out /  Change )

Twitter picture

You are commenting using your Twitter account. Log Out /  Change )

Facebook photo

You are commenting using your Facebook account. Log Out /  Change )

Connecting to %s

This site uses Akismet to reduce spam. Learn how your comment data is processed.