Python code for position control of DC motor with encoder

Support Raspberry Pi
1

I’m looking to automate the opening and closing a door weighing about 3Kg. I have tried using a 25Kg.cm servo but find that slowing the servo by moving in increments results in a jerky motor response. It seems that a 1:500 geared DC motor with an encoder might be more suitable. I have not been able to find suitable python code to control the position of such a motor.
Can somebody please refer me to some python code that could be adapted to rotate motor with encoder say 100 degrees when a signal is sent to a RPi pin, and then reverse when receiving another signal?
Thanks, Richard

1 Like
2

Hi Richard,

I’ve used 100cpt quadrature encoder on a 18:1 geared DC motors to control acceleration, velocity and position on the Roverling Mk.II platform. I ended up using an RP2040 MCU and its PIO based state machine to decode the encoder.

The following code (QueryEncoders) is executed every 5ms by a timer interrupt. The LE and RE globals will represent the positive or negative count since the last read from the left and right encoders. From this I derive velocity, but it doesn’t miss many counts so it could also be used for dead reckoning position determination as well.

The main loop reads this data asynchronously. So for you application the code can be modified to cause an interrupt when the accumulated count exceeds however many counts it takes to get to 100 degrees.

To take this further it is pretty easy to control the acceleration & max velocity so you get a very smooth start and finish.

mark

################################################################################
# Roverling Mk II - Quadrature Decoder
# Count transitions using RP2040 PIO based state machine, no CPU cycles and then
# use timer based interrupt to read accumulated counts, aka velocity. - 5ms
# Motor quadrature optical encoder: 100 CPT, 17.83:1, 1783 counts per revolution

from machine import Pin, Timer
from rp2 import asm_pio, StateMachine
from array import array
import time

REncApin = Pin(12, Pin.IN, Pin.PULL_UP)
REncBpin = Pin(13, Pin.IN, Pin.PULL_UP)  
LEncApin = Pin(10, Pin.IN, Pin.PULL_UP)
LEncBpin = Pin(11, Pin.IN, Pin.PULL_UP)  

# Initis
PrevREnc = 0
PrevLEnc = 0

dataV = array('f', [0]*2)

# Parameters
EncoderSamplePeriod = 50      
Counts2Vel = 400

@asm_pio()
def encoder():       
    # adapted from https://github.com/adamgreen/QuadratureDecoder
    # In C/C++ can define base, which needs to be 0 for jump table to work
    # in python not working, but padding out to exactly 32 instructions fixes it
    # 16 element jump table based on 4-bit encoder last state and current state.

    jmp('delta0')     # 00-00
    jmp('delta0')     # 00-01
    jmp('delta0')     # 00-10
    jmp('delta0')     # 00-11

    jmp('plus1')      # 01-00
    jmp('delta0')     # 01-01
    jmp('delta0')     # 01-10
    jmp('minus1')     # 01-11

    jmp('minus1')     # 10-00
    jmp('delta0')     # 10-01
    jmp('delta0')     # 10-10
    jmp('plus1')      # 10-11

    jmp('delta0')     # 11-00
    jmp('delta0')     # 11-01
    jmp('delta0')     # 11-10
    jmp('delta0')     # 11-11
                        
    label('delta0')     # Program actually starts here.
    mov(isr, null)      # Make sure that the input shift register is cleared when table jumps to delta0.
    in_(y, 2)           # Upper 2-bits of address are formed from previous encoder pin readings Y -> ISR[3,2]
    mov(y, pins)        # Lower 2-bits of address are formed from current encoder pin readings. PINS -> Y
    in_(y, 2)           # Y -> ISR[1,0]
    mov(pc, isr)        # Jump into jump table which will then jump to delta0, minus1, or plus1 labels.

    label('minus1')
    jmp(x_dec,'output') # Decrement x
    jmp('output')

    label('plus1')
    mov(x, invert(x))   # Increment x by calculating x=~(~x - 1)
    jmp(x_dec,'next2')
    label('next2')
    mov(x, invert(x))

    label('output')
    mov(isr, x)         #Push out updated counter.
    push(noblock)
    jmp('delta0')

    nop()                #need to pad out to exactly 32 instructions
    nop()
    nop()

RightMotorEncoder = StateMachine(0, encoder, freq=1000000, in_base=REncApin)
LeftMotorEncoder = StateMachine(1, encoder, freq=1000000, in_base=LEncApin)

def twos_comp(val, bits):
    if (val & (1 << (bits - 1))) != 0:          # if sign bit is set e.g., 8bit: 128-255
        val = val - (1 << bits)                 # compute negative value
    return val                                  # return positive value as is

def QueryEncoders(): 
    global PrevLEnc, PrevREnc
    global LeftVel, RightVel

    k = 0
    while (LeftMotorEncoder.rx_fifo() > 0) and (k < 4):    # empty FIFO - last value used
        k += 1
        LEE = LeftMotorEncoder.get()
    if k > 0:
        LEE = twos_comp(LEE, 32)
        LE = LEE - PrevLEnc
        PrevLEnc = LEE
    else:
        LE = 0

    k = 0
    while (RightMotorEncoder.rx_fifo() > 0) and (k < 4):  
        k += 1
        REE = RightMotorEncoder.get()
    if k > 0:
        REE = twos_comp(REE, 32)
        RE = REE - PrevREnc
        PrevREnc = REE
    else:
        RE = 0
    
    dataV[0], dataV[1] = LE/ Counts2Vel, RE/Counts2Vel


def cbEncSampleTimer(t):
    QueryEncoders()
    
RightMotorEncoder.active(1)
LeftMotorEncoder.active(1)

EncSampleTimer = Timer(period=EncoderSamplePeriod , mode=Timer.PERIODIC, callback=cbEncSampleTimer)

Also on GitHub

2 Likes
3

Thanks Mark, I’ll try adapting your code which is out of my league.

2 Likes
4

Hi Richard,

To get your servo working you might be able to use a bendy or stretchy linkage to smooth it out a little or add a reduction either through changing the length of the linking arm or a gear system.

Mark has some great code there and if you need a hand adapting it I’d try some AI tools or post back here and we can take a look!

Liam

5

Thanks Liam for these ideas. I’ll try these and let you know if I get an acceptable result.
Richard

1 Like