DC Motor working as a switch.

[nuno2500]

Hello PyBricks team,

I have wrote some code in order to detect movement using a DC motor. Maybe you can add this feature to Pybricks. By reading current variation i can use a cheap DC motor as a switch. Here is my code:

from pybricks.hubs import CityHub
from pybricks.pupdevices import DCMotor
from pybricks.parameters import Port
from pybricks.tools import wait
hub = CityHub()
M = DCMotor(Port.A)
M.dc(7) # Send low power so motor doesn´t move.

while True:

    c1 = hub.battery.current()
    wait(150)
    c2 = hub.battery.current()

    D =  c1- c2 #current variation

    if D > 2:
        print (D,"Right")
        wait(1000)
    
    if D < -2:
        print (D,"Left")
        wait(1000)

[laurensvalk]

Cool! But how would you tell different ports apart?

[nuno2500]

That´s why i need your help. Maybe you can implement a function that can tell port current.

[nuno2500]

The main problem is that this code is very slow a have gives errors when is "inside" a big program.

[dlech]

There is only one current sensor for the entire hub, so it isn't possible to get the motor current for an individual port.

[nuno2500]

Ok, i understand. Thanks for your answer because now i know it can´t be done so i won´t burn my small brain :)

[dlech]

Happy hacking 😄

[Debenben]

While the fundamental problem of only having one current sensor for the entire hub cannot be solved - In case you have only one active Motor attached and you are willing to tune your code to detect movement on this specific motor and hub combination it might be possible. I adapted my Kalman filter https://github.com/orgs/pybricks/discussions/1411#discussioncomment-14133925 roughly guessing the parameters and assuming a linear relationship between motor current and its speed:

from pybricks.hubs import ThisHub
from pybricks.tools import Matrix, vector
from pybricks.pupdevices import Motor
from pybricks.parameters import Port
from pybricks.tools import wait

hub = ThisHub()
M = Motor(Port.A)
M.dc(7) # Send low power so motor doesn´t move.
wait(1000) # wait for some time for motor respond

def invert2x2(M): # returns inverse of matrix M
    a = M[0,0]
    b = M[0,1]
    c = M[1,0]
    d = M[1,1]
    return 1/(a*d - b*c)*Matrix([[d, -b], [-c, a]])

# parameters:
dt = 0.004 # time between measurements = duration of while loop iteration. 0.004 for CityHub, other hubs are faster
sigma = 0.01 # uncertainty for both current and current change
variance = 0.01 # variance for both current and current change
v_thresh = 5 # current threshold for stationary detection
v_count = 5 # current number threshold for stationary detection
I = Matrix([[1, 0, 0], [0, 1, 0], [0, 0, 1]]) # unit matrix
F = Matrix([[1, dt, 0.5*dt**2], [0, 1, dt], [0, 0, 1]]) # state transistion matrix
Q = variance*dt*I # uncorrelated noise
#Q = variance*Matrix([[0.25*dt**4, 0.5*dt**3, 0.5*dt**2], [0.5*dt**3, dt**2, dt], [0.5*dt**2, dt, 1]]) # piecewise white noise

# initialize values:
x_post = vector(0, 0, 0) # initial state
P_post = sigma*I # initial covariance
c_last = hub.battery.current() # last measured current
c_average = hub.battery.current() # average measured current
stationary = 0 # count currents below threshold
count = 0 # count loop iterations

while True:
    count += 1

    # prediction:
    x_pred = F*x_post
    P_pred = F*P_post*F.T + Q

    # measurement:
    c = hub.battery.current()
    c_average = 0.0001*(c_average*9999 + c)

    a = c - c_last
    v = c_average - c
    if abs(v) < v_thresh:
        stationary += 1
    else:
        stationary = 0

    c_last = c

    H = Matrix([[0, 0, 1], [0, 1, 0]])
    if stationary > v_count:
        # motor is stationary -> acceleration and velocity 0
        z = vector(0, 0)
    else:
        # motor is moving -> velocity measured by current
        z = vector(a, v)
    S = H*P_pred*H.T + sigma*Matrix([[1, 0], [0, 1]])
    K = P_pred*H.T*invert2x2(S)

    # correction:
    x_post = x_pred + K*(z - H*x_pred)
    P_post = (I - K*H)*P_pred

    if count%100 == 0:
        # position, speed, acceleration
        print(f"{x_post[0]:+09.6f}, {x_post[1]:+09.6f}, {x_post[2]:+09.6f}")

It can predict in what direction I turn my motor and roughly get its position. In case this is useful for you feel free to give it a try.