Uncategorized

spv5 notes

By on • ( Leave a comment )

Making a Rpi Pico Based Smart Vehicle

ProjectsJun 3017 / 17Sep 715h ago1 MONTH LATER

tlfong0115 6d

SPV v50.01 Development Notes

Last reply was hitting this forum’s 30k words limit. So I am making this new reply. Let me make a briefly progress report.

(1) Progress so far

I started with a copy cat of Tom’sHardware tutorial of using a motor driver to drive a single motor (MX1508 driving TT130). Everything went OK.

I then used the TB6612FNG motor driver to drive two N20 motor. Everything also went well.

I then used the TB6612FNG motor driver to drive two TT130 motors, and I had a big problem of intermittent failures when trying to sequentially test one motor and then the other, and found that one or other motor could not start, or only could start if I used my hand to help start moving. One starts moving, the motor can move happily. It took me some 4 or 5 hours to conclude that the non start problem might be a software problem, and a software reset might solve the problem. Anyway, I tidied up the program, version 50.01 which can almost always start one motor then another motor, and the other way round. I am saving this almost very good program for later reference.

SPV50.01 Program Listing

(2) Troubleshooting by swapping

I have been using the swapping/pairing trick which I found very effective to locate the trouble making place. Very briefly, I test two motors, with two sets of hardware and wiring. So I can swap motor, connector, wring in seconds, as show in the photo below. Actually I have 4 or 5 identical sets of motors and duPont connectors for fast swapping.

spv_2wd_5001_2021sep01011024×1145 313 KB

Rpi Pico TB6612FNG TT130 Motor Schematic V5.0

pico_tb6612fng_2021sep0101799×690 137 KB

Sticky Motor 2 Cannot Always Self Start Problem

I use the following test to sequentially run Motor 1 and Motor 2 four times.

for count in range(4):
    testMeasureEncodeIntCountMoveSeconds(motorDriverDictNum = 2, motorNum = 1, pwmFreq = 1000, dutyCycle = 100, moveSeconds = 2)
    testStopMotor(motorDriverDictNum = 2, motorNum = 1)    
    utime.sleep(1)
    testMeasureEncodeIntCountMoveSeconds(motorDriverDictNum = 2, motorNum = 2, pwmFreq = 1000, dutyCycle = 100, moveSeconds = 2)
    testStopMotor(motorDriverDictNum = 2, motorNum = 2)    
    utime.sleep(1)

I still find Motor has a problem to start, and I need to push it a little bit to help it to start. I am not sure if it is the motor’s stick gear box problem, or the motor driver’s second channel sticky problem. I need to do some swapping troubleshooting tomorrow.

TT130 Encoder Motor Calibration Notes

I am going to measure the speeds of a range of motor power, 9V, 7.2V, 6V, 4.5V, and 3V. The picture below shows 9V motor power gives 70rpm.tt130_12v_2021sep0203800×480 149 KB

Now below is the complete list of Vm (V motor) vs speed (rpm)

9.0V = 350mA = 240uS = 70rpm
7.2V = 270mA = 300us = 55rpm
6.0V = 250mA = 340us = 49rpm
4.5V = 200mA = 400us = 42rpm
3.0V = 150mA = 660us = 25rpm

Now what I am curious to know or to calibrate is if I use Vm motor power = 9V, what is the corresponding (PWM freq = 1000Hz) Duty Cycle of 7.2V/55rpm, 6.0V 49rpm 4.5V/42rpm, 3.0V/25rpm?

TT130 Motor Vm vs speed (rpm)

I found speed vs Vm (yellow line) is rather linear. I also need to plot speed vs duty cycle, as I did for N20 motor earlier. So far so good. So I will take a longer break this time.tt130_rpm_vs_vm_2021sep0201869×309 125 KB

Vehicle Dictionary Config

So far we have been assigning each of the two motors by two numbers: motorDictNum, motorNum. So far so good. We have tested OK all the motor dependant functions by two numbers motorDictNum, motorNum

Now we are going the motor dictionary one level up, to vehicle dictionary vechicleDict. Each vehicleDict configs on vehicle which has two motors.

Earlier to setup two motors, we need to set two motors one by one. Now we can set up the two motors of one vehicle in one go, by just saying setup vehicle,

All the low functions stays the same, it is only the higher vehicle dictionaries and functions are new. In other words if the vehicle dicts and function are working, all the lower calling functions should work without any problem (this is the beauty of “Functional Programming”.

The new vehicle dictionaries and functions are listed below:

# *** Vehicle Config ***

vehicleDict01 = {
                  'MOTOR_DRIVER_DICT_NUM': 2,
                }

vehicleDictDict = {
                     '1': vehicleDict01,
                     '2': vehicleDict01,
                  }

# *** Vehicle Functions ***

def setupVehicle(vehicleNum):
    motorDriverDictNum = vehicleDictDict[str(vehicleNum)]['MOTOR_DRIVER_DICT_NUM']
    setupMotor(motorDriverDictNum, 1)
    setupMotor(motorDriverDictNum, 2)
    return

Vehicle Abstract Level Functions

Now I am starting off to write the first coouple of vehicle level functions. Below is an example.

# *** Old Motor Abstract Level Function ***
# Motor Level ADT (Abstract Data Type) on moving motor forward
def testMoveMotorForwardSeconds(motorDriverDictNum, motorNum, moveSeconds):
    print('\ntestmoveMotorForwardSeconds()')
    motorDict = setupMotor(motorDriverDictNum, motorNum)
    motorConfigDict = motorDict['MOTOR_CONFIG_DICT']
    moveMotorForwardSeconds(motorConfigDict, moveSeconds)
    stopMotor(motorConfigDict)     
    return

# Example call
testMoveMotorForwardSeconds(motorDriverDictNum = 2, motorNum = 1, moveSeconds = 2)

# *** Proposed New Vehicle Abstract Level Functions ***

def testVehicleMoveOneMotor(vehicleNum, wheelNum, actionNum, speedNum, moveSeconds):
    ...
    return

Progress report of writing vehicle level functions

Writing the vehicle functions has been as smooth as I expected. I first wrote a function to move a vehicle’s two front motors, then another function to stop the front two motors. The test sample below (1) run the two motors non stop, (2) hold 4 seconds, (3) stop two motors.

# *** Vehicle Config ***

vehicleDict01 = {
                  'MOTOR_DRIVER_DICT_NUM': 2,
                }

vehicleDictDict = {
                     '1': vehicleDict01,
                     '2': vehicleDict01,
                  }

# *** Vehicle and Wheel Functions ***

def setupVehicle(vehicleNum):
    motorDriverDictNum  = vehicleDictDict[str(vehicleNum)]['MOTOR_DRIVER_DICT_NUM']
    frontLeftMotorDict  = setupMotor(motorDriverDictNum, 1)
    frontRightMotorDict = setupMotor(motorDriverDictNum, 2)
    motorDictDict = {'FRONT_LEFT_MOTOR': frontLeftMotorDict, 'FRONT_RIGHT_MOTOR': frontRightMotorDict}
    return motorDictDict

def moveVehicleTwoFrontMotorsForwardNonStop(vehicleNum):
    motorDictDict = setupVehicle(vehicleNum)    
    frontLeftMotorConfigDict  = motorDictDict['FRONT_LEFT_MOTOR']['MOTOR_CONFIG_DICT']
    frontRightMotorConfigDict = motorDictDict['FRONT_RIGHT_MOTOR']['MOTOR_CONFIG_DICT']       
    moveMotorForwardNonStop(frontLeftMotorConfigDict)
    moveMotorForwardNonStop(frontRightMotorConfigDict)    
    return

def stopVehicleTwoFrontMotors(vehicleNum):
    motorDictDict = setupVehicle(vehicleNum)    
    frontLeftMotorConfigDict  = motorDictDict['FRONT_LEFT_MOTOR']['MOTOR_CONFIG_DICT']
    frontRightMotorConfigDict = motorDictDict['FRONT_RIGHT_MOTOR']['MOTOR_CONFIG_DICT']   
    stopMotor(frontLeftMotorConfigDict)
    stopMotor(frontRightMotorConfigDict)    
    return

def testMoveVehicleTwoFrontMotorsForwardNonStop():
    moveVehicleTwoFrontMotorsForwardNonStop(vehicleNum = 1)
    return

def testStopVehicleTwoFrontMotors():
    stopVehicleTwoFrontMotors(vehicleNum = 1)
    return

# *** Sample Test ***

# testMoveVehicleTwoFrontMotorsForwardNonStop()
# utime.sleep(4)
# testStopVehicleTwoFrontMotors()

Move vehicle forward and backward, turn vehicle left and right tested OK

Only four statements are need to move vehicle forward and backward, turn left and right, as listed below:

# test move vehicle forward and backward, turn vehicle left and right
testMoveVehicleMotorListForwardHoldTime()   # move forward  4 seconds
testMoveVehicleMotorListBackwardHoldTime()  # move backward 4 seconds
testTurnVehicleLeftHoldTime()               # turn left     4 seconds
testTurnVehicleRightHoldTime()              # turn right    4 seconds

Full listing below:

# Program Name
#   spv5011_2021sep0504.py - tlfong01 2021sep05hkt1927

# 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 Aspire XC-780 Intel CORE i5-6400 2.7GHz, 8GB, Chinese Windows 10 Home (64-bit, 2020nov15),
#   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
#   TT130 1:48 gear motor 
#     9.0V = 350mA = 240uS = 70rpm
#     7.2V = 270mA = 300us = 55rpm
#     6.0V = 250mA = 340us = 49rpm
#     4.5V = 200mA = 400us = 42rpm
#     3.0V = 150mA = 660us = 25rpm
#   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

# Brief Description of Program Function
#   1. Move DC motor forward, backware, stop.
#   2. Use PWM to control motor speed
#   3. Use motor encoder to measure speed
#   4. Use encoder signals to help driving a 2WD to move in a straight line

# Digital Oscilloscope ATTEN ADS1102CAL+
# https://toolboom.com/en/digital-oscilloscope-atten-ads1102calplus/

# ATTEN ADS100 User Manual
# https://micromir.ucoz.ru/Oscil/Atten/ADS1000_User_Manual.pdf

import utime
from machine import Pin, PWM

# ========= ========= ========= ========= ========= ========= ========= =========
# ========= ========= ========= ========= ========= ========= ========= =========
# ========= ========= ========= ========= ========= ========= ========= =========
# ========= ========= ========= ========= ========= ========= ========= =========

# *** Motor Level Config and Functions ***

# *** Motor Dicts ***

motorDriverDict02 = {
              'TITLE' : 'TB6612FNG Dual DC Motor Driver Dictionary v0.2  tlfong01 2021aug23hkt1009',
              'STANDBY' : 5, 
              '1': {'IN1': 10, 'IN2': 11, 'PWM' : 3, 'ENCODE': 14, \
                    'DEFAULT_PWM_FREQ': 1000, 'DEFAULT_DUTY_CYCLE': 50, 'DEFAULT_ENCODE_INT_PULSE_PER_REVOLUTION': 980},
              '2': {'IN1': 12, 'IN2': 13, 'PWM' : 4, 'ENCODE': 15, \
                    'DEFAULT_PWM_FREQ': 1000, 'DEFAULT_DUTY_CYCLE': 50, 'DEFAULT_ENCODE_INT_PULSE_PER_REVOLUTION': 980},
              '3': {'IN1':  0, 'IN2':  0, 'PWM' : 0, 'ENCODE':  0, \
                    'DEFAULT_PWM_FREQ': 1000, 'DEFAULT_DUTY_CYCLE': 50, 'DEFAULT_ENCODE_INT_PULSE_PER_REVOLUTION': 980},
              '4': {'IN1':  0, 'IN2':  0, 'PWM' : 0, 'ENCODE':  0, \
                    'DEFAULT_PWM_FREQ': 1000, 'DEFAULT_DUTY_CYCLE': 50, 'DEFAULT_ENCODE_INT_PULSE_PER_REVOLUTION': 980},                 
              }

motorDriverDictDict = {
                '1': motorDriverDict02,
                '2': motorDriverDict02,                
                }

# *** Motor Functions ***

def setupMotor(motorDriverDictNum, motorNum):
    motorDriverDict   = motorDriverDictDict[str(motorDriverDictNum)] # Get driver dict
    driverStandBy     = Pin(motorDriverDict['STANDBY'], Pin.OUT) # create driverStandBy pin object    
    motorIn1          = Pin(motorDriverDict[str(motorNum)]['IN1'], Pin.OUT) # Create Motor # motorNum In1 pin object
    motorIn2          = Pin(motorDriverDict[str(motorNum)]['IN2'], Pin.OUT) # Create Motor # motorNum In2 pin object    
    motorPwm          = PWM(Pin(motorDriverDict[str(motorNum)]['PWM'])) # Create Motor # motorNum Pwm pin object    
    motorEncode       = Pin(motorDriverDict[str(motorNum)]['ENCODE'], Pin.IN, Pin.PULL_DOWN) # Create Motor # motorNum Encode pin object
  
    motorPwmFreq      = motorDriverDict[str(motorNum)]['DEFAULT_PWM_FREQ']
    motorDutyCycle    = motorDriverDict[str(motorNum)]['DEFAULT_DUTY_CYCLE']
    motorEncIntCntRev = motorDriverDict[str(motorNum)]['DEFAULT_ENCODE_INT_PULSE_PER_REVOLUTION']
    
    motorConfigDict = {'StdBy': driverStandBy, 'In1': motorIn1, 'In2': motorIn2, 'Pwm': motorPwm, 'Encode': motorEncode}    
    
    motorStateDict = {'PwmFreq': motorPwmFreq, 'DutyCycle': motorDutyCycle, 'EncodeIntCntRev': motorEncIntCntRev} 
    
    motorDict = {'MOTOR_CONFIG_DICT': motorConfigDict, 'MOTOR_STATE_DICT': motorStateDict}
    
    motorConfigDict['StdBy'].high() # enable motor driver normal operation, (low = disable)
    motorConfigDict['Pwm'].freq(motorStateDict['PwmFreq'])                          # setup frequency
    motorConfigDict['Pwm'].duty_u16(int(65536 / 100) * motorStateDict['DutyCycle']) #   and duty cycle
    stopMotor(motorConfigDict)
    return motorDict

def changeMotorPwmFreqAndDutyCycle(motorDict, pwmFreq, dutyCycle):
    motorStateDict = motorDict['MOTOR_STATE_DICT']
    motorStateDict['PwmFreq'] = pwmFreq
    motorStateDict['DutyCycle'] = dutyCycle
    
    motorConfigDict = motorDict['MOTOR_CONFIG_DICT']
    motorConfigDict['Pwm'].freq(motorStateDict['PwmFreq'])                          
    motorConfigDict['Pwm'].duty_u16(int(65536 / 100) * motorStateDict['DutyCycle'])
    return

def stopMotor(motorConfigDict):
    motorConfigDict['In1'].low() 
    motorConfigDict['In2'].low() 
    return

def moveMotorForwardNonStop(motorConfigDict):
    motorConfigDict['In1'].low()  # move motor
    motorConfigDict['In2'].high() #   backward
    return

def moveMotorForwardSeconds(motorConfigDict, moveSeconds):
    moveMotorForwardNonStop(motorConfigDict)
    utime.sleep(moveSeconds)    
    stopMotor(motorConfigDict)
    return

def moveMotorForwardHoldTime(motorConfigDict, holdTime):
    moveMotorForwardNonStop(motorConfigDict)
    utime.sleep(holdTime)    
    stopMotor(motorConfigDict)
    return

def moveMotorBackwardNonStop(motorConfigDict):
    motorConfigDict['In1'].high()  # move motor
    motorConfigDict['In2'].low() #   backward
    return

def moveMotorBackwardSeconds(motorConfigDict, moveSeconds):
    moveMotorBackwardNonStop(motorConfigDict)    
    utime.sleep(moveSeconds)    
    stopMotor(motorConfigDict)
    return

# ========= ========= ========= ========= ========= ========= ========= =========
# ========= ========= ========= ========= ========= ========= ========= =========
# ========= ========= ========= ========= ========= ========= ========= =========
# ========= ========= ========= ========= ========= ========= ========= =========

# *** Interrupt Related Functions ***

def encodeIntCallBack(pin):
    global encodeIntCount
    encodeIntCount = encodeIntCount + 1    
    return

def readEncodeIntCount(motorDriverDictNum, motorNum, pwmFreq, dutyCycle, moveSeconds):
    motorDict = setupMotor(motorDriverDictNum, motorNum)   
    motorStateDict  = motorDict['MOTOR_STATE_DICT']
    motorConfigDict = motorDict['MOTOR_CONFIG_DICT']
    changeMotorPwmFreqAndDutyCycle(motorDict, pwmFreq, dutyCycle)    

    motorConfigDict['Encode'].irq(encodeIntCallBack, Pin.IRQ_FALLING)
    
    global encodeIntCount
    encodeIntCount = 0    
    
    moveMotorForwardNonStop(motorConfigDict)  
    utime.sleep(moveSeconds)        
    stopMotor(motorConfigDict)              
    
    totalEncodeIntCount = encodeIntCount
    return totalEncodeIntCount

# *** Move revoloutions and distance ***

def moveMotorEncodeIntCount(motorDriverDictNum, motorNum, encodeIntCount):
    motorDict = setupMotor(motorDriverDictNum, motorNum)
    motorConfigDict = motorDict['MOTOR_CONFIG_DICT']    
        
    #intCountPerSecond = 983
    intCountPerSecond = 750     
  
    print('  encodeIntCount    =', encodeIntCount)
    print('  intCountPerSecond =', intCountPerSecond)
 
    moveSeconds = encodeIntCount / intCountPerSecond  
    print('  moveSeconds       =', moveSeconds)      
       
    moveMotorForwardNonStop(motorConfigDict)
    utime.sleep(moveSeconds)
    stopMotor(motorConfigDict)
    return

def moveMotorRevolutions(motorDriverDictNum, motorNum, revolutions):
    motorDict = setupMotor(motorDriverDictNum, motorNum)
    motorConfigDict = motorDict['MOTOR_CONFIG_DICT']    
        
    intCountPerRevolution = 983
    intCountPerSecond = 750
    moveSeconds = (revolutions * intCountPerRevolution) / intCountPerSecond     
  
    print('  intCountPerRevolution    =', intCountPerRevolution)
    print('  intCountPerSecond        =', intCountPerSecond)
    print('  moveSeconds              =', moveSeconds)      
       
    moveMotorForwardNonStop(motorConfigDict)
    utime.sleep(moveSeconds)
    stopMotor(motorConfigDict)
    return

# *** Test Functions *** 

def testMoveMotorForwardSeconds(motorDriverDictNum, motorNum, moveSeconds):
    print('\ntestmoveMotorForwardSeconds()')
    motorDict = setupMotor(motorDriverDictNum, motorNum)
    motorConfigDict = motorDict['MOTOR_CONFIG_DICT']
    moveMotorForwardSeconds(motorConfigDict, moveSeconds)
    stopMotor(motorConfigDict)     
    return

def testMoveMotorBackwardSeconds(motorDriverDictNum, motorNum, moveSeconds):
    print('\ntestMoveMotorBackwardSeconds()')
    motorDict = setupMotor(motorDriverDictNum, motorNum)
    motorConfigDict = motorDict['MOTOR_CONFIG_DICT']
    moveMotorBackwardSeconds(motorConfigDict, moveSeconds)
    stopMotor(motorConfigDict)     
    return

def testChangeMotorPwmFreqAndDutyCycleAndmoveMotorForwardSeconds(motorDriverDictNum, motorNum, pwmFreq, dutyCycle, moveSeconds):
    print('\ntestChangeMotorPwmFreqAndDutyCycleAndmoveMotorForwardSeconds()')
    motorDict = setupMotor(motorDriverDictNum, motorNum)
    changeMotorPwmFreqAndDutyCycle(motorDict, pwmFreq, dutyCycle)
    
    motorConfigDict = motorDict['MOTOR_CONFIG_DICT']
    print('moveSeconds =', moveSeconds)
    moveMotorForwardSeconds(motorConfigDict, moveSeconds)
    stopMotor(motorConfigDict)     
    return 

def testMeasureMotorSpeed(motorDriverDictNum, motorNum, pwmFreq, dutyCycle):    
    totalEncodeIntCount = readEncodeIntCount(motorDriverDictNum, motorNum, pwmFreq, dutyCycle, moveSeconds = 1)        
    print('motorNum =', motorNum, end = '')
    print('  pwmFreq   =', pwmFreq, end = '')
    print('  dutyCycle =', dutyCycle, end = '')
    print('  totalEncodeIntCount Per Second =', totalEncodeIntCount, end = '')
    print('  motor speed rpm =', int(int(totalEncodeIntCount * 60)/100))              
    return

def testMoveMotorEncodeIntCount(motorDriverDictNum, motorNum, encodeIntCount):
    print('testMoveMotorEncodeIntCount(), ...')
    moveMotorEncodeIntCount(motorDriverDictNum, motorNum, encodeIntCount)
    return

def testMoveMotorRevolutions(motorDriverDictNum, motorNum, revolutions):
    print('testMoveMotorRevolutions(), ...')
    moveMotorRevolutions(motorDriverDictNum, motorNum, revolutions)
    return

def testMeasureEncodeIntCountMoveSeconds(motorDriverDictNum, motorNum, pwmFreq, dutyCycle, moveSeconds):
    encodeIntCount = readEncodeIntCount(motorDriverDictNum, motorNum, pwmFreq, dutyCycle, moveSeconds)
    print('MotorDriverDictNum =', motorDriverDictNum, ' ', end = '')
    print('MotorNum =', motorNum, ' ', end = '')
    print('PWM Freq =', pwmFreq, ' ', end = '')
    print('DutyCycle =', dutyCycle, ' ', end = '')
    print('MoveSeconds =,', moveSeconds, ' ', end = '')            
    print('encodeIntCount =', encodeIntCount)
    return encodeIntCount

def testStopMotor(motorDriverDictNum, motorNum):
    motorDict = setupMotor(motorDriverDictNum, motorNum)
    motorConfigDict = motorDict['MOTOR_CONFIG_DICT']
    stopMotor(motorConfigDict)     
    return

# *** Old Main Tests ***

'''
testMoveMotorForwardSeconds(motorDriverDictNum = 2, motorNum = 1, moveSeconds = 2)
testMoveMotorBackwardSeconds(motorDriverDictNum = 2, motorNum = 2, moveSeconds = 4)
testChangeMotorPwmFreqAndDutyCycleAndmoveMotorForwardSeconds(motorDriverDictNum = 2, motorNum = 1 , \                                              pwmFreq = 1000, dutyCycle = 10, moveSeconds = 2)                               
testChangeMotorPwmFreqAndDutyCycleAndmoveMotorForwardSeconds(motorDriverDictNum = 2, motorNum = 1 , \ 
                                                     pwmFreq = 1000, dutyCycle = 90, moveSeconds = 1)
testMeasureMotorSpeed(motorDriverDictNum = 2, motorNum = 1, pwmFreq = 1000, dutyCycle = 80)
testMeasureMotorSpeed(motorDriverDictNum = 2, motorNum = 1, pwmFreq = 1000, dutyCycle = 10)
testMoveMotorEncodeIntCount(motorDriverDictNum = 2, motorNum = 1, encodeIntCount = 1020)
testMoveMotorEncodeIntCount(motorDriverDictNum = 2, motorNum = 2, encodeIntCount = 1920)
testMoveMotorRevolutions(motorDriverDictNum = 2, motorNum = 1, revolutions = 1)
testMeasureEncodeIntCountMoveSeconds(motorDriverDictNum = 2, motorNum = 1, pwmFreq = 1000, dutyCycle = 100, moveSeconds = 4)
testStopMotor(motorDriverDictNum = 2, motorNum = 1)
'''

# ========= ========= ========= ========= ========= ========= ========= =========
# ========= ========= ========= ========= ========= ========= ========= =========
# ========= ========= ========= ========= ========= ========= ========= =========
# ========= ========= ========= ========= ========= ========= ========= =========

#
# *** Vehicle Level Config and Functions ***

vehicleDict01 = {
                  'MOTOR_DRIVER_DICT_NUM': 2,
                }

vehicleDictDict = {
                     '1': vehicleDict01,
                     '2': vehicleDict01,
                  }

# *** Vehicle and Wheel Functions ***

def setupVehicle(vehicleNum):
    motorDriverDictNum  = vehicleDictDict[str(vehicleNum)]['MOTOR_DRIVER_DICT_NUM']
    frontLeftMotorDict  = setupMotor(motorDriverDictNum, 1)
    frontRightMotorDict = setupMotor(motorDriverDictNum, 2)
    motorDictDict = {'FRONT_LEFT_MOTOR': frontLeftMotorDict, 'FRONT_RIGHT_MOTOR': frontRightMotorDict}
    return motorDictDict

def moveVehicleMotorListForwardHoldTime(vehicleNum, holdTime):
    motorDictDict = setupVehicle(vehicleNum)    
    frontLeftMotorConfigDict  = motorDictDict['FRONT_LEFT_MOTOR']['MOTOR_CONFIG_DICT']
    frontRightMotorConfigDict = motorDictDict['FRONT_RIGHT_MOTOR']['MOTOR_CONFIG_DICT']       
    motorConfigDictList = [frontLeftMotorConfigDict,  frontRightMotorConfigDict] 
    for motorConfigDict in motorConfigDictList:
        moveMotorForwardNonStop(motorConfigDict)
    utime.sleep(holdTime)
    for motorConfigDict in motorConfigDictList:
       stopMotor(motorConfigDict) 
    return

def moveVehicleMotorListBackwardHoldTime(vehicleNum, holdTime):
    motorDictDict = setupVehicle(vehicleNum)    
    frontLeftMotorConfigDict  = motorDictDict['FRONT_LEFT_MOTOR']['MOTOR_CONFIG_DICT']
    frontRightMotorConfigDict = motorDictDict['FRONT_RIGHT_MOTOR']['MOTOR_CONFIG_DICT']       
    motorConfigDictList = [frontLeftMotorConfigDict,  frontRightMotorConfigDict] 
    for motorConfigDict in motorConfigDictList:
        moveMotorBackwardNonStop(motorConfigDict)
    utime.sleep(holdTime)
    for motorConfigDict in motorConfigDictList:
       stopMotor(motorConfigDict) 
    return

def turnTwoMotorsLeftHoldTime(frontLeftMotorConfigDict, frontReghtConfigDict, holdTime):
    moveMotorForwardNonStop(frontLeftMotorConfigDict)
    moveMotorBackward(frontRightMotorConfigDict)    
    utime.sleep(holdTime)    
    stopMotor(frontLeftMotorConfigDict)
    stopMotor(frontRightMotorConfigDict)    
    return

def turnVehicleLeftHoldTime(vehicleNum, holdTime):
    motorDictDict = setupVehicle(vehicleNum)    
    frontLeftMotorConfigDict  = motorDictDict['FRONT_LEFT_MOTOR']['MOTOR_CONFIG_DICT']
    frontRightMotorConfigDict = motorDictDict['FRONT_RIGHT_MOTOR']['MOTOR_CONFIG_DICT']       
    moveMotorForwardNonStop(frontLeftMotorConfigDict)
    moveMotorBackwardNonStop(frontRightMotorConfigDict)    
    utime.sleep(holdTime)
    stopMotor(frontLeftMotorConfigDict)
    stopMotor(frontRightMotorConfigDict)    
    return

def turnVehicleRightHoldTime(vehicleNum, holdTime):
    motorDictDict = setupVehicle(vehicleNum)    
    frontLeftMotorConfigDict  = motorDictDict['FRONT_LEFT_MOTOR']['MOTOR_CONFIG_DICT']
    frontRightMotorConfigDict = motorDictDict['FRONT_RIGHT_MOTOR']['MOTOR_CONFIG_DICT']       
    moveMotorBackwardNonStop(frontLeftMotorConfigDict)
    moveMotorForwardNonStop(frontRightMotorConfigDict)    
    utime.sleep(holdTime)
    stopMotor(frontLeftMotorConfigDict)
    stopMotor(frontRightMotorConfigDict)    
    return

# *** Test Functions ***

def testMoveVehicleMotorListForwardHoldTime():
    moveVehicleMotorListForwardHoldTime(vehicleNum = 1, holdTime = 4)
    return

def testMoveVehicleMotorListBackwardHoldTime():
    moveVehicleMotorListBackwardHoldTime(vehicleNum = 1, holdTime = 4)
    return

def testTurnVehicleLeftHoldTime():
    turnVehicleLeftHoldTime(vehicleNum = 1, holdTime = 4)
    return

def testTurnVehicleRightHoldTime():
    turnVehicleRightHoldTime(vehicleNum = 1, holdTime = 4)
    return

# ========= ========= ========= ========= ========= ========= ========= =========
# ========= ========= ========= ========= ========= ========= ========= =========
# ========= ========= ========= ========= ========= ========= ========= =========
# ========= ========= ========= ========= ========= ========= ========= =========

# *** Main Tests ***

# * test move vehicle forward and backward, turn vehicle left and right
testMoveVehicleMotorListForwardHoldTime()   # move forward  4 seconds
testMoveVehicleMotorListBackwardHoldTime()  # move backward 4 seconds
testTurnVehicleLeftHoldTime()               # turn left     4 secopnds
testTurnVehicleRightHoldTime()              # turn right    4 seconds
    
# End

Now the youtube.

pico 2wd test 2021sep0501

How to sync the two motors

We can see that the two motors do not sync, in the sense that the two wheels, each with the white stripe (cable tie), should move at the same speed.

In other words, if they start being adjacent to each other, they should always stay adjacent after any number of revolutions, forward or backward. And for turning left and right, they should go opposite direction, but stay adjacent again (for a split second) after one revolution.

I have not yet any solid idea of how to do the sync. It would be nice if any interested readers can reply with suggestions.

My first version of interrupt function works OK, but only for one motor. Now I am trying to modify it for use in 2WD and 4WD. The first draft is listed below.

# *** Interrupt Functions - Draft v0.1 2021sep06hkt1632 ***

# Design Notes

# 1. I am drafting the interrupt functions for 2WD, scalable to 4WD
# 2. / to continue, ...

def setupMotorIntCallBacks(VehicleNum = 1):
    # draft only!!! 
    #motorConfigDict = motorDict['MOTOR_CONFIG_DICT']
    #motorStateDict  = motorDict['MOTOR_STATE_DICT']
    #motorConfigDict['Encode'].irq(encodeIntCallBack, Pin.IRQ_FALLING)
    return 

def motorEncodeIntCallBack1(pin):
    global motorEncodeIntCount1
    motorEncodeIntCount1 = motorEncodeIntCount1 + 1
    return

def motorEncodeIntCallBack2(pin):
    global motorEncodeIntCount2
    motorEncodeIntCount2 = motorEncodeIntCount2 + 1
    return

def motorEncodeIntCallBack3(pin):
    global motorEncodeIntCount3
    motorEncodeIntCount3 = motorEncodeIntCount3 + 1
    return

def motorEncodeIntCallBack4(pin):
    global motorEncodeInt4
    motorEncodeIntCount4 = motorEncodeIntCount4 + 1
    return

def readMotorEncodeIntEventCount1(motorDict, countTime):    
    global motorEncodeInt1
    return motorEncodeInt1

def readMotorEncodeIntEventCount2(motorDict, countTime):    
    global motorEncodeInt2
    return motorEncodeInt2

def readMotorEncodeIntEventCount3(motorDict, countTime):    
    global motorEncodeInt3
    return motorEncodeInt3

def readMotorEncodeIntEventCount4(motorDict, countTime):    
    global motorEncodeInt4
    return motorEncodeInt4

I am hitting the forum’s 32k limit. So I am making another reply to move on, …

/ to continue, …Reply

tlfong012 15h

I am drafting a test function to proving the concept of:

  1. Using a list of two interrupt event counts of this 2WD (later 4 counts for 4WD).
  2. Using only one callback function for 2WD’s two interrupt pins.
# *** Interrupt Functions ***

# *** Interrupt Event Variables and Service Routines *** 

# *** Setup Motor and Vehicle ***

def setupMotorV2(motorDriverDictNum, motorNum):
    motorDriverDict         = motorDriverDictDict[str(motorDriverDictNum)] # Get driver dict
    driverStandBy           = Pin(motorDriverDict['STANDBY'], Pin.OUT) # create driverStandBy pin object    
    motorIn1                = Pin(motorDriverDict[str(motorNum)]['IN1'], Pin.OUT) # Create Motor # motorNum In1 pin object
    motorIn2                = Pin(motorDriverDict[str(motorNum)]['IN2'], Pin.OUT) # Create Motor # motorNum In2 pin object    
    motorPwm                = PWM(Pin(motorDriverDict[str(motorNum)]['PWM'])) # Create Motor # motorNum Pwm pin object    
    motorEncode             = Pin(motorDriverDict[str(motorNum)]['ENCODE'], Pin.IN, Pin.PULL_DOWN) # Create Motor # motorNum Encode pin object
    motorPwmFreq            = motorDriverDict[str(motorNum)]['DEFAULT_PWM_FREQ']
    motorDutyCycle          = motorDriverDict[str(motorNum)]['DEFAULT_DUTY_CYCLE']
    motorEncodeIntCount     = 0   
    motorConfigDict = {'StdBy': driverStandBy, 'In1': motorIn1, 'In2': motorIn2, 'Pwm': motorPwm, 'Encode': motorEncode}
    motorConfigDict['StdBy'].high() # enable motor driver normal operation, (low = disable)
    motorConfigDict['Pwm'].freq(motorPwmFreq)                          # setup frequency
    motorConfigDict['Pwm'].duty_u16(int(65536 / 100) * motorDutyCycle) #   and duty cycle    
    motorStateDict = {'PwmFreq': motorPwmFreq, 'DutyCycle': motorDutyCycle, 'MotorEncodeIntCount': motorEncodeIntCount}     
    motorDict = {'MOTOR_CONFIG_DICT': motorConfigDict, 'MOTOR_STATE_DICT': motorStateDict}    
    stopMotor(motorConfigDict)
    return motorDict

def setupVehicleV2(vehicleNum):
    print('  setupVehicle(), ...')
    motorDriverDictNum  = vehicleDictDict[str(vehicleNum)]['MOTOR_DRIVER_DICT_NUM']
    frontLeftMotorDict  = setupMotorV2(motorDriverDictNum, 1)
    frontRightMotorDict = setupMotorV2(motorDriverDictNum, 2)
    frontLeftMotorInterruptPin  = frontLeftMotorDict['MOTOR_CONFIG_DICT']['Encode']
    frontRightMotorInterruptPin = frontRightMotorDict['MOTOR_CONFIG_DICT']['Encode']
    motorInterruptPinList = [frontLeftMotorInterruptPin, frontRightMotorInterruptPin]    
    vehicleDict = {'FRONT_LEFT_MOTOR': frontLeftMotorDict, 'FRONT_RIGHT_MOTOR': frontRightMotorDict, 'INTERRUPT_PIN_LIST': motorInterruptPinList}  
    return vehicleDict

def testSyncTwoVehicleWheels():
    print('testSyncTwoVehicleWheels(), ...')   
  
    # *** This function reads one of the interrupt count list *** 
    def readMotorEncodeIntCount(countNum):    
        return motorEncodeIntCount[countNum]

    # *** motorEncodeCallBack ***
    # Notes:
    #   This callback function is a bit too clever to understand.  All motor interrupt pins will trigger this callback.
    #   The callback will find which interrupt pin interrupts, then increment the corresponding element of the int count list.    
    def motorEncodeIntCallBack(pin):
        index = interruptPinList.index(pin)
        motorEncodeIntCountList[index] += 1    
        return        
    
    vehicleDict = setupVehicleV2(vehicleNum = 1)
    
    vehicleDict['INTERRUPT_PIN_LIST'][0].irq(motorEncodeIntCallBack, Pin.IRQ_FALLING)    
    vehicleDict['INTERRUPT_PIN_LIST'][1].irq(motorEncodeIntCallBack, Pin.IRQ_FALLING)     
    
    #print('  motorEncodeIntServiceRoutine[2] =', motorEncodeIntCountList[2])    
    
    return

# *** Main Tests ***

#testMoveVehicleForwardBackwardTurnLeftTurnRight()

testSyncTwoVehicleWheels()
    
# End

I was jealous that PiTop4 can use mpu6050 gesture to control a robot. This morning I read how to do it. So I might try later to use my smart phone to control my smart robot, to say, first walk in a straight line.

Raspberry Pi Pico Controls Robot with Smartphone Accelerometer – Ash Hill, 2021sep07
Tom’s Hardware – 6 Sep 21

Raspberry Pi Pico Controls Robot with Smartphone Accelerometer

It’s like Kirby Tilt and Tumble but without Kirby and your Game Boy is a phone.

Raspberry Pi: Python Libraries for I2C, SPI, UART – Sebastian Günther, 2021sep06
DEV Community

Raspberry Pi: Python Libraries for I2C, SPI, UART

The Raspberry Pi is one of the most popular single board computers for hobbyists. Its 40 Pins support…

I feel jealous that some smart guy has a smart 4WD, while mine is only a stupid looking 2WD, which should damage my reputation. So I am stalling my 2WD project and go build a smart 4WD. Stay tuned. See you later.four_wheel_drive_2021sep07021192×751 229 KBReply

Categories: Uncategorized

Leave a Reply

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

Gravatar
WordPress.com Logo

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

Facebook photo

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

Cancel

Connecting to %s

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