Robo Věda

Web nejen o robotice

Digitální měření lineárních motorů a jejich odchylky

Reklama:

Pro měření rychlosti malých lineárních/robotických motorů bez interního enkodéru existují následující možnosti. Rozdělení je provedeno pro měření bez zátěže a se zátěží, s variantami pro různé cenové a přesnostní úrovně.

Měření bez zátěže

Nejlevnější varianta: Měření zpětné elektromotorické síly (Back EMF)

  • Princip: Využívá napětí indukované v motoru při vypnutém PWM signálu, které je úměrné rychlosti1 2.
  • Schéma:
Motor ─┬─ PWM rizeni (napr. H-mustek)  
       └─ ADC pin (s delicem napeti pro ochranu)

MicroPython kód:

from machine import ADC, PWM, Pin
import time

motor_pwm = PWM(Pin(15))
adc = ADC(Pin(26))

def measure_speed():
    motor_pwm.duty_u16(0)  # Vypnuti PWM
    time.sleep_us(100)     # Cekani na ustaleni
    emf = adc.read_u16()   # Mereni napeti
    motor_pwm.duty_u16(32768)  # Obnoveni PWM (50 %)
    return emf * 0.0001  # Kalibracni konstanta

# Mereni v obou smerech
pwm_values = [32768, 0]  # Dopredu, dozadu
for pwm in pwm_values:
    motor_pwm.duty_u16(pwm)
    print("Rychlost:", measure_speed())
  • Výhody: Nulové náklady na hardware1.
  • Nevýhody: Nízká přesnost (±10 %), citlivost na šum2.

Nejpreciznější varianta: Optický enkodér s externím kotoučem

  • Princip: Reflexní optický senzor detekuje otáčky pomocí perforovaného kotouče3 4.
  • Schéma:
Motor ── Kotouc s otvory ── Opticky senzor (napr. TCRT5000) ── GPIO

MicroPython kód:

from machine import Pin, Timer
import time

sensor = Pin(16, Pin.IN)
counter = 0

def tick(pin):
    global counter
    counter += 1

sensor.irq(trigger=Pin.IRQ_RISING, handler=tick)

def calculate_rpm():
    global counter
    start_time = time.ticks_ms()
    time.sleep(1)
    rpm = (counter * 60) / 20  # 20 otvoru na kotouci
    counter = 0
    return rpm

# Test obou smeru
for direction in [1, -1]:
    set_motor_direction(direction)  # Funkce pro zmenu smeru
    print("RPM:", calculate_rpm())
  • Výhody: Přesnost ±0.5 %4.
  • Nevýhody: Nutnost mechanické instalace kotouče3.

Optimální střed: Hallův senzor s magnetem

  • Princip: Magnet na hřídeli aktivuje Hallův senzor při každé otáčce5.
  • Schéma:
Motor ── Magnet na hrideli ── Halluv senzor (A3144) ── GPIO

MicroPython kód:

from machine import Pin, PWM
import time

hall = Pin(17, Pin.IN)
motor_pwm = PWM(Pin(15))
rotations = 0

def count_rotation(pin):
    global rotations
    rotations += 1

hall.irq(trigger=Pin.IRQ_FALLING, handler=count_rotation)

def measure_speed(duration=1):
    global rotations
    rotations = 0
    time.sleep(duration)
    return rotations / duration * 60  # RPM

# Mereni pro oba smery
for duty in [32768, 49152]:  # 50 % a 75 % vykonu
    motor_pwm.duty_u16(duty)
    print("RPM:", measure_speed())
  • Výhody: Kompromis mezi cenou a přesností (±2 %)5.
  • Nevýhody: Vyžaduje montáž magnetu.

Měření se zátěží (robotické vozítko)

Nejlevnější varianta: Kalibrace pomocí PWM a času jízdy

  • Princip: Měření driftu vozítka při jízdě po definované trati a korekce PWM6 7.

  • Postup:

  • Nastavte oba motory na stejné PWM

  • Změřte odchylku dráhy po 1 m
  • Upravte PWM poměr: nové_PWM = původní_PWM ± (odchylka * korekční_koeficient)

Kód pro korekci:

def calibrate_motors():
    Kp = 0.1  # Korekcni konstanta
    base_speed = 30000
    left_pwm = base_speed
    right_pwm = base_speed

    for _ in range(5):  # 5 mereni
        deviation = measure_deviation()  # Funkce pro mereni odchylky
        left_pwm += int(deviation * Kp)
        right_pwm -= int(deviation * Kp)

    return left_pwm, right_pwm

Nejpreciznější varianta: PID regulace s optickými enkodéry

  • Princip: Průběžná korekce rychlosti pomocí zpětné vazby z enkodérů8 9.
  • Schéma:
[Opticky enkoder] ── GPIO  
                  ── PID regulator ── PWM

PID kód:

from machine import PWM, Pin
import time

class PID:
    def __init__(self, Kp, Ki, Kd):
        self.Kp, self.Ki, self.Kd = Kp, Ki, Kd
        self.integral = 0
        self.last_error = 0

    def update(self, error):
        self.integral += error
        derivative = error - self.last_error
        output = self.Kp*error + self.Ki*self.integral + self.Kd*derivative
        self.last_error = error
        return output

pid_left = PID(0.8, 0.05, 0.01)
pid_right = PID(0.8, 0.05, 0.01)

while True:
    left_rpm = get_left_rpm()  # Z enkoderu
    right_rpm = get_right_rpm()
    error = left_rpm - right_rpm

    correction = pid_left.update(error)
    set_pwm(left_motor, base_speed + correction)
    set_pwm(right_motor, base_speed - correction)

Optimální střed: Hallovy senzory s adaptivním PWM

  • Princip: Dynamická úprava PWM na základě rozdílu v počtech impulsů7.
  • Kód:
def adaptive_control():
    left_count = hall_left.count()
    right_count = hall_right.count()

    ratio = left_count / (right_count + 1e-6)  # Aby nedochazelo k deleni nulou
    if ratio > 1.1:
        adjust_pwm(right_motor, +5%)
    elif ratio < 0.9:
        adjust_pwm(left_motor, +5%)

Univerzální měřící skript pro MicroPython

from machine import Pin, PWM, ADC, Timer
import time

# Konfigurace hardwaru
motor_left = PWM(Pin(13))
motor_right = PWM(Pin(14))
sensor_left = Pin(15, Pin.IN)
sensor_right = Pin(16, Pin.IN)

# Promenne pro mereni
count_left, count_right = 0, 0

def count_left(pin):
    global count_left
    count_left +=1

def count_right(pin):
    global count_right
    count_right +=1

sensor_left.irq(trigger=Pin.IRQ_RISING, handler=count_left)
sensor_right.irq(trigger=Pin.IRQ_RISING, handler=count_right)

def measure_speed(duration=1):
    global count_left, count_right
    count_left = count_right = 0
    time.sleep(duration)
    return (count_left/duration, count_right/duration)

# Testovaci sekvence
for speed in [0.3, 0.5, 0.7]:
    motor_left.duty_u16(int(65535*speed))
    motor_right.duty_u16(int(65535*speed))
    left, right = measure_speed()
    print(f"Korekce: {left-right} pulses/s")

Toto řešení kombinuje jednoduchost měření s možností kalibrace pro různé provozní podmínky. Pro robotická vozítka doporučujeme implementovat PID regulátor s optickými enkodéry pro maximální přesnost8 9.

Citovaná díla

  1. https://www.precisionmicrodrives.com/ab-021 ↩↩

  2. http://qa.orientalmotor.com/brushless-dc-motors-gear-motors/technology/speed-control-methods-of-speed-control-motors.html ↩↩

  3. https://www.electroschematics.com/motor-speed-sensor-module-circuit/ ↩↩

  4. https://www.edn.com/motor-speed-sensor-module-circuit/ ↩↩

  5. https://www.instructables.com/RPM-Measurement-Using-Hall-Sensor-and-Arduino/ ↩↩

  6. https://www.vexforum.com/t/driving-straight/17779 ↩

  7. https://thetechnicgear.com/2014/06/howto-calibrate-differential-drive-robot/ ↩↩

  8. https://wseas.com/journals/amcse/2021/amcsejournal2021-010.pdf ↩↩

  9. https://www.heidenhain.us/resources-and-news/linear-encoders-for-linear-motors-for-direct-drives/ ↩↩