Program Name
measure_motor_speed_v15.py – tlfong01 2021aug18hkt1832
Reference
Pi-Top Forum – making-a-rpi-pico-based-smart-vehicle/924
https://forum.pi-top.com/t/making-a-rpi-pico-based-smart-vehicle/924
#
Configuration
Acer Intel COIRE i5 PC Chinese Windows 10 (64-bit), Thonny 3.3.3, Python 3.7.9 (32-bit), Tk 8.6.9, USB COM Port #4
#
Thonny MicroPython Intepreter
Micropython (Rapsberry Pi Pico)
#
DC Motor
N20 1:100 6V DC Gear Motor with quadrature encoder signals A, B
N20 Gear Motor Spec (6V gear 1:100 no load speed = 300 rpm (https://www.pololu.com/search/compare/173)
DC Motor Driver
TB6612FNG Dual DC Motor Driver
Program Function
Measure speed of DC motor N20 with quadrature encoder
import utime
from machine import Pin, PWM
*** Configuration ***
motorDriverDict01 = {
‘TITLE’ : ‘TB6612FNG Dual DC Motor Driver Dictionary’,
‘STANDBY’ : 5,
‘1’: {‘IN1’: 10, ‘IN2’: 11, ‘PWM’ : 3, ‘ENCODE’: 14},
‘2’: {‘IN1’: 12, ‘IN2’: 13, ‘PWM’ : 4, ‘ENCODE’: 15},
‘3’: {‘IN1’: 0, ‘IN2’: 0, ‘PWM’ : 0, ‘ENCODE’: 0},
‘4’: {‘IN1’: 0, ‘IN2’: 0, ‘PWM’ : 0, ‘ENCODE’: 0},
}
motorDriverDictDict = {
‘1’: motorDriverDict01,
‘2’: motorDriverDict01,
}
*** Setup Motor Driver, Move Motor Forward, Backward, Stop ***
def setupMotorDriver(motorDriverDictNum):
motorDriverDict = motorDriverDictDict[str(motorDriverDictNum)]
standBy = Pin(motorDriverDict[‘STANDBY’], Pin.OUT)
standBy.high()
return
def setupMotor(motorDriverDictNum, motorNum):
motorDriverDict = motorDriverDictDict[str(motorDriverDictNum)]
motorIn1 = Pin(motorDriverDict[str(motorNum)][‘IN1’], Pin.OUT)
motorIn2 = Pin(motorDriverDict[str(motorNum)][‘IN2’], Pin.OUT)
motorPwm = Pin(motorDriverDict[str(motorNum)][‘PWM’], Pin.OUT)
motorEncode = Pin(motorDriverDict[str(motorNum)][‘ENCODE’], Pin.IN, Pin.PULL_DOWN)
motorControlPinList = [motorIn1, motorIn2, motorPwm, motorEncode]
return motorControlPinList
def moveMotorForward(motorDriverDictNum, motorNum):
motorControlPinList = setupMotor(motorDriverDictNum, motorNum)
motorControlPinList[0].low()
motorControlPinList[1].high()
motorControlPinList[2].high()
return
def moveMotorBackward(motorDriverDictNum, motorNum):
motorControlPinList = setupMotor(motorDriverDictNum, motorNum)
motorControlPinList[0].high()
motorControlPinList[1].low()
motorControlPinList[2].high()
return
def moveMotorStop(motorDriverDictNum, motorNum):
motorControlPinList = setupMotor(motorDriverDictNum, motorNum)
motorControlPinList[0].low()
motorControlPinList[1].low()
motorControlPinList[2].high()
return
*** Read Motor Encoder, Measure Motor Speed ***
def readMotorEncodeValue(motorDriverDictNum, motorNum):
motorControlPinList = setupMotor(motorDriverDictNum, motorNum)
encodeValue = motorControlPinList[3].value()
return encodeValue
def measureMotorSpeed(motorDriverDictNum, motorNum):
# *** Setup Motor Driver ***
setupMotorDriver(motorDriverDictNum)
# *** Move motor forward ***
moveMotorForward(motorDriverDictNum, motorNum)
# *** Measure Motor Speed ***
# *** Start counting 10 revolutions ***
usTicks1 = utime.ticks_us()
# *** Find lapse time of 100 revolutions ***
revCount = 0
while revCount < 10:
if readMotorEncodeValue(motorDriverDictNum, motorNum) == 0:
revCount = revCount + 1
utime.sleep(0.000001)
usTicks2 = utime.ticks_us()
rev10Us = utime.ticks_diff(usTicks2, usTicks1)
revUs = int(rev10Us / 10)
rps = int(1000000 / revUs)
rpmRaw = int(rps * 60)
rpmGear100 = int(rpmRaw / 100)
print(' uS per revolution =', revUs)
print(' rps raw =', rps)
print(' rpm raw =', rpmRaw)
print(' rpm gear 1:100 =', rpmGear100)
return rpmGear100
*** PWM Functions ***
def setupMotorDriverPwm(motorDriverDictNum):
motorDriverDict = motorDriverDictDict[str(motorDriverDictNum)]
# print(‘ motDriverDictNum =’, motorDriverDictNum)
standBy = Pin(motorDriverDict[‘STANDBY’], Pin.OUT)
standBy.high()
return
def setupMotorPwm(motorDriverDictNum, motorNum, pwmFreq, dutyCycle):
motorDriverDict = motorDriverDictDict[str(motorDriverDictNum)]
motorIn1Pin = motorDriverDict[str(motorNum)][‘IN1’]
motorIn2Pin = motorDriverDict[str(motorNum)][‘IN2’]
motorPwmPin = motorDriverDict[str(motorNum)][‘PWM’]
# print(' IN1 pinNum =', motorIn1Pin)
# print(' IN2 pinNum =', motorIn2Pin)
# print(' PWM pinNum =', motorPwmPin)
motorIn1 = Pin(motorIn1Pin, Pin.OUT)
motorIn2 = Pin(motorIn2Pin, Pin.OUT)
motorPwm = PWM(Pin(motorDriverDict[str(motorNum)]['PWM']))
motorPwm.freq(pwmFreq)
motorPwm.duty_u16(int(65536 / 100) * dutyCycle)
pwmPinNum = motorDriverDict[str(motorNum)]['PWM']
# print(' Setup PWM')
# print(' PWM freq (Hz) =', motorPwm.freq())
# print(' PWM duty (%) =', int((motorPwm.duty_u16() / 65536) * 100))
# print(' PWM width (ns) =', motorPwm.duty_ns())
motorEncode = Pin(motorDriverDict[str(motorNum)]['ENCODE'], Pin.IN, Pin.PULL_DOWN)
motorControlPinList = [motorIn1, motorIn2, motorPwm, motorEncode]
return motorControlPinList
def moveMotorForwardPwm(motorDriverDictNum, motorNum, pwmFreq, dutyCycle):
motorControlPinList = setupMotorPwm(motorDriverDictNum, motorNum, pwmFreq, dutyCycle)
motorControlPinList[0].low()
motorControlPinList[1].high()
motorControlPinList[2].freq(pwmFreq)
motorControlPinList[2].duty_u16(int(65536 / 100) * dutyCycle)
return
def measureMotorSpeedPwm(motorDriverDictNum, motorNum):
# *** Setup Motor Driver ***
# setupMotorDriver(motorDriverDictNum)
# *** Move motor forward ***
# moveMotorForwardPwm(motorDriverDictNum, motorNum, pwmFreq, dutyCycle)
# *** Measure Motor Speed ***
# *** Start counting 10 revolutions ***
usTicks1 = utime.ticks_us()
# *** Find lapse time of 100 revolutions ***
revCount = 0
while revCount < 10:
if readMotorEncodeValue(motorDriverDictNum, motorNum) == 0:
revCount = revCount + 1
utime.sleep(0.0001) # <<<<<<<<<<<<
usTicks2 = utime.ticks_us()
rev10Us = utime.ticks_diff(usTicks2, usTicks1)
revUs = int(rev10Us / 10)
rps = int(1000000 / revUs)
rpmRaw = int(rps * 60)
rpmGear100 = int(rpmRaw / 100)
# print(' uS per revolution =', revUs)
# print(' rps raw =', rps)
# print(' rpm raw =', rpmRaw)
# print(' rpm gear 1:100 =', rpmGear100)
return rpmGear100
*** Motor Test Functions ***
def testMoveMotor01(motorDriverDictNum, motorNum):
# *** Setup Driver ***
setupMotorDriver(motorDriverDictNum)
# *** Move motor forward ***
moveMotorForward(motorDriverDictNum, motorNum)
utime.sleep(1)
# *** Move motor backward ***
moveMotorBackward(motorDriverDictNum, motorNum)
utime.sleep(1)
# *** Move motor stop ***
moveMotorStop(motorDriverDictNum, motorNum)
return
def testMeasureMotorSpeed01(motorDriverDictNum, motorNum):
measureMotorSpeed(motorDriverDictNum, motorNum)
return
def testSetupMotorPwm01(motorDriverDictNum, motorNum, pwmFreq, dutyCycle):
setupPwm(motorDriverDictNum, motorNum, pwmFreq, dutyCycle)
return
def testMoveMotorForwardPwm01(motorDriverDictNum, motorNum, pwmFreq, dutyCycle):
setupMotorDriverPwm(motorDriverDictNum)
moveMotorForwardPwm(motorDriverDictNum, motorNum, pwmFreq, dutyCycle)
utime.sleep(4)
moveMotorStop(motorDriverDictNum, motorNum)
return
def testMeasureMotorSpeedPwm01(motorDriverDictNum, motorNum, pwmFreq, dutyCycle):
setupMotorDriverPwm(motorDriverDictNum)
moveMotorForwardPwm(motorDriverDictNum, motorNum, pwmFreq, dutyCycle)
utime.sleep(2)
speedRpmGear = measureMotorSpeedPwm(motorDriverDictNum, motorNum)
print(‘dutyCycle =’, dutyCycle)
print(‘speed (rpmGear) =’, speedRpmGear)
# moveMotorStop(motorDriverDictNum, motorNum)
return
*** Main Tests ***
*** Test move motor forward, backward, stop ***
print(‘Test Motor #1 Move Forward, Backward, Stop’)
testMoveMotor01(motorDriverDictNum = 1, motorNum = 1)
print(‘\nTest Motor #2 Move Forward, Backward, Stop’)
testMoveMotor01(motorDriverDictNum = 1, motorNum = 2)
*** Test measure motor speed ***
print(‘\nTest Measure Motor #1 Speed’)
testMeasureMotorSpeed01(motorDriverDictNum = 1, motorNum = 1)
*** Test Move Motor / Measure Speed PWM Funcions ***
print(‘Test Move Motor Forward’)
testMoveMotorForwardPwm01(motorDriverDictNum = 1, motorNum = 1, pwmFreq = 1000, dutyCycle = 100)
testMoveMotorForwardPwm01(motorDriverDictNum = 1, motorNum = 1, pwmFreq = 1000, dutyCycle = 50)
testMoveMotorForwardPwm01(motorDriverDictNum = 1, motorNum = 1, pwmFreq = 1000, dutyCycle = 10)
print(‘Test Measure Motor Speed’)
testMeasureMotorSpeedPwm01(motorDriverDictNum = 1, motorNum = 1, pwmFreq = 1000, dutyCycle = 80)
print(‘Test Measure Motor Speed’)
testMeasureMotorSpeedPwm01(motorDriverDictNum = 1, motorNum = 1, pwmFreq = 1000, dutyCycle = 50)
print(‘Test Measure Motor Speed’)
testMeasureMotorSpeedPwm01(motorDriverDictNum = 1, motorNum = 1, pwmFreq = 1000, dutyCycle = 40)
print(‘Stop Motor’)
moveMotorStop(motorDriverDictNum = 1, motorNum = 1)
*** End of program ***
*** Sample Output – tlfong01 2021aug18hkt1120
”’
%Run -c $EDITOR_CONTENT
revUs = 1250
rps = 800
rpm raw = 48000
rpm 100 = 480”’
”’
%Run -c $EDITOR_CONTENT
Test PWM Funcions
PWM freq (Hz) = 1000
PWM duty (%) = 49
PWM width (ns) = 499728”’
Categories: Uncategorized
tlfong01.blog 