Uncategorized

motor speed measurement

Program Name

measure_motor_speed_v18.py – tlfong01 2021aug19hkt2151

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

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

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

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

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

*** N20 Motor Speed Measurement Function Using Two Core Threads With Interrupts v0.1 ***

led_dirPin = machine.Pin(27, machine.Pin.IN,machine.Pin.PULL_DOWN)

led_dirPin.irq(trigger=machine.Pin.IRQ_RISING, handler=led_dirTask)

encodePin1Num = 6
encodePin2Num = 7

encodePin1 = machine.Pin(encodePin1Num, machine.Pin.IN, machine.Pin.PULL_DOWN)

def encodeIntHandler():
global encodIntCount
enIntCount + 1
return

encodePin1.irq(trigger = machine.Pin.IRQ_FALLING, handler = encodeIntHandler)

”’

*** Old Speed Measurement Functions ***

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

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

def testMeasureMotorSpeed01(motorDriverDictNum, motorNum):
measureMotorSpeed(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

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(‘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)

*** Test Move Motor / Measure Speed PWM Funcions ***

*** Test measure motor speed ***

print(‘\nTest Measure Motor #1 Speed’)

testMeasureMotorSpeed01(motorDriverDictNum = 1, motorNum = 1)

”’

*** 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 testMoveMotorForwardPwm01(motorDriverDictNum, motorNum, pwmFreq, dutyCycle):
setupMotorDriverPwm(motorDriverDictNum)
moveMotorForwardPwm(motorDriverDictNum, motorNum, pwmFreq, dutyCycle)
utime.sleep(4)
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)

*** Test move motor forward, backward, stop ***

print(‘\nTest Motor #2 Move Forward, Backward, Stop’)
testMoveMotor01(motorDriverDictNum = 1, motorNum = 2)

*** Test move motor forward PWM ***

print(‘\nTest Move Motor Forward With Different Duty Cycles’)
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(‘Stop Motor’)

moveMotorStop(motorDriverDictNum = 1, motorNum = 1)

*** End of program ***

*** Sample Output – tlfong01 2021aug19hkt2150

”’
”’

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.