Uncategorized

spv2

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

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.