C3PO – 4WD Motorsteuerung mit Cytron Maker Drive

Der Cytron Maker Drive hat mich einige Nerven gekostet. Soviel schon mal vorweg. Damit dieses Board funktioniert, muss mindestens ein Ground Pin des Raspis mit einem Ground von einer Motorsteuerung verbunden sein, da die PWM Pins sonst keine Erdung haben. Es reicht einer (auch, wenn man zwei Cytron Boards verbaut hat).

Raspi an Erde

Ich hatte hier Sensoren und den ESP32 NodeMCU angeschlossen und wollte diese Motor-Steuerung schon fast in den Müll werfen, da sich die Räder einfach nicht drehen wollten.

Kinderleicht war die Inbetriebnahme der Motorsteuerung am Ende meiner Meinung nach dann eher nicht, da Cytron vorschlägt mit pigpio und dem pigpiod Daemon zu arbeiten (Link: https://abyz.me.uk/rpi/pigpio/)

Unter Debian Buster lässt sich pigpio über:

sudo apt install pigpio
sudo systemctl start pigpiod

installieren.

Unter Ubuntu, wer hätte es gedacht, etwas komplizierter, kann man die Anleitung unter: https://abyz.me.uk/rpi/pigpio/download.html durchführen und den Dienst manuell mit: sudo pigpiod starten.

Der pigpiod Dienst stellt daraufhin Python einen Software PWM zur Verfügung, sodass man mehr als 4 PWM Pins zur Verfügung hat – im Fall von 4 Motoren also 8:

Acht PWM Pins mit pigpio

Die Basis-Klasse für die Motorsteuerung des C3PO Robot-Car musste ich also entsprechend anpassen:

# motorpwm.py
import time

import pigpio

class MotorController:
    _max_speed = 255
    _default_speed = 100
    _set_debug = False

    def __init__(self, default_speed: int = _default_speed, max_speed: int = None, debug: bool = _set_debug):
        self._motors = {}
        self._default_speed = default_speed
        self._pi = pigpio.pi()
        self._debug = debug
        if max_speed is None:
            self._max_speed = MotorController._max_speed
        else:
            self._max_speed = max_speed

    def get_debug(self):
        return self._debug

    def get_default_speed(self):
        return self._default_speed

    def get_last_direction(self):
        return self._default_speed

    def get_max_speed(self):
        return self._max_speed

    def get_pi(self):
        return self._pi


    def add_motor(self, name: str, IN1: int, IN2: int, front: bool = None, left: bool = None):
        self._motors[name] = {}
        self._motors[name]["IN1"] = IN1
        self._motors[name]["IN2"] = IN2
        self._motors[name]["CURRENTSPEED"] = 0
        self._motors[name]["FORWARDS"] = True
        self._motors[name]["NAME"] = name
        self._motors[name]["FRONT"] = front
        self._motors[name]["LEFT"] = left

    def get_motor(self, name: str):
        return self._motors[name]

    def get_motors(self):
        return self._motors

    def start_motor(self):
        for motor in self.get_motors():
            mo = self.get_motor(motor)
            M2A = mo["IN1"]
            M2B = mo["IN2"]
            self.get_pi().set_mode(M2A, pigpio.OUTPUT)
            self.get_pi().set_mode(M2B, pigpio.OUTPUT)

    def car_move_direction(self, speed: int = _default_speed, forwarddirection: bool = True, duration: int = 0):
        motors = self.get_motors()
        try:
            for name in motors:
                self.set_motor_direction(name, forwarddirection)
                self.set_motor_control_speed(name, speed)
            if duration > 0:
                time.sleep(duration)
                self.stop()
        except:
            self.stop()
            raise
                    
    def car_move_forwards(self, speed: int = _default_speed, duration: int = 0):
        if speed is None:
            speed = self.get_default_speed()
        self.car_move_direction(speed, True, duration)

    def car_move_backwards(self, speed: int = None, duration: int = 0):
        if speed is None:
            speed = self.get_default_speed()
        self.car_move_direction(speed, False, duration)

    def set_motor_speed(self, name, speed):
        self._motors[name]["CURRENTSPEED"] = speed

    def get_motor_speed(self, name):
        return self._motors[name]["CURRENTSPEED"]

    def set_motor_control_speed(self, name, speed: int =_default_speed):
        if speed > self.get_max_speed():
            speed = self.get_max_speed()
        self.set_motor_speed(name, speed)
        motor = self.get_motor(name)
        M2A = motor["IN1"]
        M2B = motor["IN2"]
        if motor["FORWARDS"]:
            if self.get_debug():
                print("Set duty - Forwards: " ,name, speed, M2A, M2B)
            self.get_pi().set_PWM_dutycycle(M2A, speed)
            self.get_pi().set_PWM_dutycycle(M2B, 0)
        else:
            if self.get_debug():
                print("Set duty - Backwards: " ,name, speed)
            self.get_pi().set_PWM_dutycycle(M2B, speed)
            self.get_pi().set_PWM_dutycycle(M2A, 0)

    def set_motor_direction(self, name: str, forward: bool):
        motor = self.get_motor(name)
        motor["FORWARDS"] = forward

    def accelerate_car(self, acceleration: int = 1):
        for motor in self.get_motors():
            self.accelerate_motor(motor, acceleration)

    def brake_car(self, brake: int = 1):
        for motor in self.get_motors():
            self.brake_motor(motor, brake)

    def accelerate_motor(self, name: str, acceleration: int = 1):
        current_speed = self.get_motor_speed(name)
        new_speed = current_speed + acceleration
        if new_speed > self.get_max_speed():
            new_speed = self.get_max_speed()
        self.set_motor_control_speed(name, new_speed)

    def brake_motor(self, name: str, brake: int = 1):
        current_speed = self.get_motor_speed(name)
        new_speed = current_speed - brake
        if new_speed < 0:
            new_speed = 0
        self.set_motor_control_speed(name, new_speed)

    def stop(self):
        motors = self.get_motors()
        for name in motors:
            self.set_motor_control_speed(name, 0)
        # Auslaufen der Räder
        time.sleep(0.1)

Die Four Wheels Klasse ist zwar neu, aber fast identisch mit der Mecanum Steuerungs Klasse. Das hielt sich in Grenzen:

# four_wheels_motor.py

import time

from robotbench.motor.motorpwm import MotorController

class FourWheelsMotorController(MotorController):

    def car_move_leftright_direction(self, speed: int = None, forwarddirection: bool = True, leftdirection: bool = True, duration: int = 0):
        if speed is None:
            speed = self.get_default_speed()
        motors = self.get_motors()
        try:
            for name in motors:
                mo = self.get_motor(name)
                print("Motor: ", name)
                if forwarddirection:
                    if mo["LEFT"] and leftdirection:
                        self.set_motor_direction(name, True)
                        self.set_motor_control_speed(name, speed)
                    if not mo["LEFT"] and not leftdirection:
                        self.set_motor_direction(name, True)
                        self.set_motor_control_speed(name, speed)
                else:
                    if mo["LEFT"] is not leftdirection:
                        self.set_motor_direction(name, False)
                        self.set_motor_control_speed(name, speed)
                    if not mo["LEFT"] and leftdirection:
                        self.set_motor_direction(name, False)
                        self.set_motor_control_speed(name, speed)
            
            if duration > 0:
                print("sleep", duration)
                time.sleep(duration)
                self.stop()
        except:
            self.stop()
            raise

    def car_move_right(self, speed: int = None, forwarddirection: bool = True, duration: int = 0):
        if speed is None:
            speed = self.get_default_speed()
        self.car_move_leftright_direction(speed, forwarddirection, False, duration)

    def car_move_left(self, speed: int = None, forwarddirection: bool = True, duration: int = 0):
        if speed is None:
            speed = self.get_default_speed()
        self.car_move_leftright_direction(speed, forwarddirection, True, duration)

    def car_move_forward_right(self, speed: int = None, duration: int = 0):
        if speed is None:
            speed = self.get_default_speed()
        self.car_move_right(speed, True, duration)

    def car_move_forward_left(self, speed: int = None, duration: int = 0):
        if speed is None:
            speed = self.get_default_speed()
        self.car_move_left(speed, True, duration)

    def car_move_backwards_right(self, speed: int = None, duration: int = 0):
        if speed is None:
            speed = self.get_default_speed()
        self.car_move_right(speed, False, duration)

    def car_move_backwards_left(self, speed: int = None, duration: int = 0):
        if speed is None:
            speed = self.get_default_speed()
        self.car_move_left(speed, False, duration)

Und noch ein kleines Test Programm, um zu sehen, ob alles läuft:

# motor_test.py

from robotbench.motor.four_wheels_motor import FourWheelsMotorController

WD4 = FourWheelsMotorController(debug=True)

WD4.add_motor("LEFT_FRONT", 22, 10, True, True)
WD4.add_motor("RIGHT_FRONT", 27, 17, True, False)
WD4.add_motor("LEFT_BACK", 9, 11, False, True)
WD4.add_motor("RIGHT_BACK", 13, 6, False, False)

try:
    WD4.start_motor()
    WD4.car_move_left(120, True, 1)
    WD4.car_move_left(120, False, 1)
    WD4.car_move_right(120, True, 1)
    WD4.car_move_right(120, False, 1)
except:
    pass

WD4.stop()

Falls man bei den Anschlüssen etwas falsch gemacht und wie ich, den Boden nicht nochmal abschrauben will, kann man die Motoren und deren Eigenschaften hier einfach umbenennen und die Pins anpassen.

Ergänzung vom 20.02.2022

Da es bei den DC-Motoren tatsächlich nicht so genau sein muss mit der PWM (Puls-Weiten-Modulation), reicht eigentlich auch die Standardansteuerung mit dem RPi.GPIO Modul (Achtung max_speed ist dann 100)l:

import time

import RPi.GPIO as gpio


class MotorController:
    _max_speed = 100
    _default_speed = 40
    _set_debug = False
    _PWM_frequency = 200


    def __init__(self, default_speed: int = _default_speed, max_speed: int = None, frequency: int = None, debug: bool = _set_debug):
        self._motors = {}
        self._default_speed = default_speed
        self._debug = debug
        if frequency is None:
            self._frequency = MotorController._PWM_frequency
        else:
            self._frequency = frequency
        if max_speed is None:
            self._max_speed = MotorController._max_speed
        else:
            self._max_speed = max_speed
            if self._max_speed > MotorController._max_speed:
                self._max_speed = MotorController._max_speed
        gpio.setwarnings(False)

    def get_debug(self):
        return self._debug

    def get_default_speed(self):
        return self._default_speed

    def get_last_direction(self):
        return 0

    def get_max_speed(self):
        return self._max_speed

    def add_motor(self, name: str, IN1: int, IN2: int, front: bool = None, left: bool = None):
        self._motors[name] = {}
        self._motors[name]["IN1"] = IN1
        self._motors[name]["IN2"] = IN2
        self._motors[name]["CURRENTSPEED"] = 0
        self._motors[name]["FORWARDS"] = True
        self._motors[name]["NAME"] = name
        self._motors[name]["FRONT"] = front
        self._motors[name]["LEFT"] = left

    def get_motor(self, name: str):
        return self._motors[name]

    def get_motors(self):
        return self._motors

    def start_motor(self):
        gpio.setmode(gpio.BCM)
        for motor in self.get_motors():
            mo = self._motors[motor]
            gpio.setup(mo["IN1"], gpio.OUT)
            gpio.setup(mo["IN2"], gpio.OUT)
            self._motors[motor]["MotorControl1"] = gpio.PWM(mo["IN1"], self._frequency)
            self._motors[motor]["MotorControl2"] = gpio.PWM(mo["IN2"], self._frequency)
            self._motors[motor]["MotorControl1"].start(0)
            self._motors[motor]["MotorControl2"].start(0)

    def car_move_direction(self, speed: int = _default_speed, forwarddirection: bool = True, duration: int = 0):
        motors = self.get_motors()
        try:
            for name in motors:
                self.set_motor_direction(name, forwarddirection)
                self.set_motor_control_speed(name, speed)
            if duration > 0:
                time.sleep(duration)
                self.stop()
        except:
            self.stop()
            self.stop_motor()
            raise
                    
    def car_move_forwards(self, speed: int = None, duration: int = 0):
        if speed is None:
            speed = self.get_default_speed()
        self.car_move_direction(speed, True, duration)

    def car_move_backwards(self, speed: int = None, duration: int = 0):
        if speed is None:
            speed = self.get_default_speed()
        self.car_move_direction(speed, False, duration)

    def set_motor_speed(self, name, speed):
        self._motors[name]["CURRENTSPEED"] = speed

    def get_motor_control1(self, name):
        return self._motors[name]["MotorControl1"]

    def get_motor_control2(self, name):
        return self._motors[name]["MotorControl2"]

    def get_motor_speed(self, name):
        return self._motors[name]["CURRENTSPEED"]

    def set_motor_control_speed(self, name, speed: int =_default_speed):
        if speed > self.get_max_speed():
            speed = self.get_max_speed()
        self.set_motor_speed(name, speed)
        motor = self.get_motor(name)
        control1 = self.get_motor_control1(name)
        control2 = self.get_motor_control2(name)
        if motor["FORWARDS"]:
            if self.get_debug():
                print("Set duty - Forwards: ", name, speed)
            control1.ChangeDutyCycle(speed)
            control2.ChangeDutyCycle(0)
        else:
            if self.get_debug():
                print("Set duty - Backwards: " ,name, speed)
            control2.ChangeDutyCycle(speed)
            control1.ChangeDutyCycle(0)

    def set_motor_direction(self, name: str, forward: bool):
        motor = self.get_motor(name)
        motor["FORWARDS"] = forward

    def accelerate_car(self, acceleration: int = 1):
        for motor in self.get_motors():
            self.accelerate_motor(motor, acceleration)

    def brake_car(self, brake: int = 1):
        for motor in self.get_motors():
            self.brake_motor(motor, brake)

    def accelerate_motor(self, name: str, acceleration: int = 1):
        current_speed = self.get_motor_speed(name)
        new_speed = current_speed + acceleration
        if new_speed > self.get_max_speed():
            new_speed = self.get_max_speed()
        self.set_motor_control_speed(name, new_speed)

    def brake_motor(self, name: str, brake: int = 1):
        current_speed = self.get_motor_speed(name)
        new_speed = current_speed - brake
        if new_speed < 0:
            new_speed = 0
        self.set_motor_control_speed(name, new_speed)

    def stop(self):
        motors = self.get_motors()
        for name in motors:
            self.set_motor_control_speed(name, 0)
        # Auslaufen der Räder
        time.sleep(0.1)

    def stop_motor(self):
        gpio.cleanup()

Ich denke, ich bleibe dann doch bei der Standardbibliothek und spare mir die 7% Dauerauslastung des Prozessors durch den pigpiod Demon.

Gut gefällt mir an der Cytron Motorsteuerung, dass Sie anscheinend anstandslos die 8×1.4V AA Batterien in Serie vertragen und doch recht günstig ist: https://www.berrybase.de/sensoren-module/motor-robotik/cytron-maker-drive-h-br-252-cken-motor-treiber. Der 5V Output, den ich für das ESP32 Board verwenden wollte, scheint nicht sehr stabil zu sein, sodass die verbundenen Infrarot-Abstandssensoren ständig Störeffekte aufwiesen.

Zum Schluss noch ein aktuelles Bild vom C3PO Robot-Car:

C3PO – jetzt auch motorisiert.

Diese Seite verwendet Cookies, um die Nutzerfreundlichkeit zu verbessern. Mit der weiteren Verwendung stimmen Sie dem zu.

Datenschutzerklärung