Kapitel 06: Motorsteuerung

: Nils Bott; Proframmierung NEW; 09. April 2019; Zugriffe: 1046

Kapitel 06: Motorsteuerung

Hinweis: Wie die einzelnen Hardware Komponenten so wie die benötigten evtl. weiteren elektronischen Zusatzteile (Widerstände, Transistoren, Stromversorgung) angeschlossen werden, ist in der Rubrik Elektronik nachzuschlagen. In dieser Rubrik wird ausschließlich die Software-Entwicklung betrachtet.

Die Motorsteuerung soll per Tastendruck möglich sein. Dies ist jedoch hier erst von sekundärer Bedeutung. Da wir uns eine ebene Tiefer an der Hardware befinden, werden wir hier nur die einzelnen Richtungen Programmieren. Die Konvertierung von Tastendruck zu Steuerungsbefehl (FORWARD, BACKWARD,RIGHT...) wird in der Java-Applikation dann später vorgenommen.

Für die Motoren kommen normale DC-Motoren zum Einsatz. Damit muss eine eigene Motorsteuerung implementiert werden. Um verschieden Drehgeschwindigkeiten und Drehrichtungen zu realisieren werden pro Motor drei Pins des RaspberryPi benötigt. Somit werden für beide Motoren 6-Pins benötigt.

Die Drehgeschwindigkeit wird dabei über ein PWM Signal gesteuert. Es handelt sich dabei um einen prozentualen Anteil im Wertebereich von 0-100. Die zwei anderen Pins dienen der Stromzufuhr, welche je nach Drehrichtung auf HIGH oder LOW gesetzt werden können.

Es sind 2 Motoren Verbaut:

Motor MA
- rechter Antriebsmotor
- GPIOS:
  - 17 - PWM Signal, 27 und 22 für Stromzufuhr, je nach Drehrichtung auf LOW oder HIGH zu setzen.
Motor MB
- linker Antriebsmotor
- GPIOS:
  - 13 - PWM Signal, 5 und 6 für Stromzufuhr, je nach Drehrichtung auf LOW oder HIGH zu setzen.

Der RaspberryPi bietet die Mögichkeit beliebige GPIO Pins dafür zu verwenden. Sprich jeder GPIO kann als PWM geber dienen oder das HIGH/LOW Signal auspielen. Es sollte jedoch immer darauf geachtet werden, keine sonder GPIO's zu verwenden, wie z.B. die I2C GPIO's, da man sich sonst evtl. Erweiterungen verbaut.

Die Folgende Tabelle gibt einen kleinen Ausblick auf die Ansteuerrung der Motoren in den vier Grundrichtungen:

GPIO	FORWARD	BACKWARD	LEFT	RIGHT
Motor MA
9	HIGH	LOW	HIGH	LOW
10	LOW	HIGH	LOW	HIGH
Motor MB
8	HIGH	LOW	LOW	HIGH
25	LOW	HIGH	HIGH	LOW

Das PWM-Signal kann dabei z.B. konstant auf dem Wert 50 bleiben, was zur halben Dreh-Geschwindigkeit des Motors bedeutet. Für Kurven muss das PWM Signal für eine Seite hoch gesetzt und für die andere Seite ggf. heruntergesetzt werden, so das unterschiedliche Drehzahlen pro Seite erreicht werden. Folglich fährt der Roboter dann in einer Kurve.

Bei uns ist dies mit einer Konstante gelöst, die "self.deffForCurve". Dieser Wert wird für eine Kurve dem PWM-Signal hinzugezogen oder abgezogen, so dass der Roboter eine Kurve fährt.

Der Programmcode für die Motoren sollte nun verständlich sein:

import time
import asyncio
import sys, getopt
import RPi.GPIO as GPIO

GPIO.setmode(GPIO.BCM)

GPIO_PWM_LEFT = 13
GPIO_LEFT_V = 5
GPIO_LEFT_GRND = 6

GPIO_PWM_RIGHT = 17
GPIO_RIGHT_V = 27
GPIO_RIGHT_GRND = 22

class DC_Motor:

#FORWARD, BACKWARD, RIGHT, LEFT, CURVERIGHTFORWARD, CURVERIGHBACKWARD, CURVELEFTBACKWARD, CURVELEFTFORWARD, STOP


    def __init__(self):

        global GPIO_PWM_LEFT
        global GPIO_LEFT_V
        global GPIO_LEFT_GRND

        global GPIO_PWM_RIGHT
        global GPIO_RIGHT_V
        global GPIO_RIGHT_GRND

        #Pins Motor A -> MotorLeft
        GPIO.setup(GPIO_PWM_LEFT, GPIO.OUT)#PWM
        GPIO.setup(GPIO_LEFT_V, GPIO.OUT) # V - Forward, High
        GPIO.setup(GPIO_LEFT_GRND, GPIO.OUT) # Grnd - Forward, LOW

        #Pins Motor B -> MotorRight
        GPIO.setup(GPIO_PWM_RIGHT, GPIO.OUT) #PWM
        GPIO.setup(GPIO_RIGHT_V, GPIO.OUT) # V -Forward, High
        GPIO.setup(GPIO_RIGHT_GRND, GPIO.OUT) # Grnd - Forward, LOW
        #PWM for Controll
        self.PWM_LEFT = GPIO.PWM(GPIO_PWM_LEFT, 100)
        self.PWM_RIGHT = GPIO.PWM(GPIO_PWM_RIGHT, 100)

        pause = 0.01 # time between pwn signal
        self.PWM_LEFT.start(0)
        self.PWM_RIGHT.start(0)
        #currentDirection
        self.speed = 0
        self.diffForCurve = 40

    def setMotorOrder(self,direction,speed):
        self.speed = float(speed)
        if direction == 'FORWARD':
            self.forward()
        elif direction == 'BACKWARD':
            self.backward()
        elif direction == 'RIGHT':
            self.right()
        elif direction == 'LEFT':
            self.left()
        elif direction == 'CURVERIGHTFORWARD':
            self.curverightforward()
        elif direction == 'CURVERIGHBACKWARD':
            self.curverighbackward()
        elif direction == 'CURVELEFTBACKWARD':
            self.curveleftbackward()
        elif direction == 'CURVELEFTFORWARD':
            self.curveleftforward()
        elif direction == 'STOP':
            self.stop()
        else:
            print ('Directio not Implement')


    def stop(self):
        self.PWM_LEFT.ChangeDutyCycle(0)
        self.PWM_RIGHT.ChangeDutyCycle(0)
        GPIO.output(GPIO_LEFT_V, GPIO.LOW)
        GPIO.output(GPIO_RIGHT_V, GPIO.LOW)
        GPIO.output(GPIO_LEFT_GRND, GPIO.LOW)
        GPIO.output(GPIO_RIGHT_GRND, GPIO.LOW)


    def forward(self):
        GPIO.output(GPIO_LEFT_V, GPIO.HIGH)
        GPIO.output(GPIO_RIGHT_V, GPIO.HIGH)
        GPIO.output(GPIO_LEFT_GRND, GPIO.LOW)
        GPIO.output(GPIO_RIGHT_GRND, GPIO.LOW)
        self.PWM_LEFT.ChangeDutyCycle(self.speed)
        self.PWM_RIGHT.ChangeDutyCycle(self.speed)

    def backward(self):
        GPIO.output (GPIO_LEFT_V, GPIO.LOW)
        GPIO.output (GPIO_RIGHT_V, GPIO.LOW)
        GPIO.output (GPIO_LEFT_GRND, GPIO.HIGH)
        GPIO.output (GPIO_RIGHT_GRND, GPIO.HIGH)
        self.PWM_LEFT.ChangeDutyCycle( self.speed)
        self.PWM_RIGHT.ChangeDutyCycle( self.speed)


    def left(self):
        GPIO.output (GPIO_LEFT_V, GPIO.HIGH)
        GPIO.output (GPIO_RIGHT_V, GPIO.LOW)
        GPIO.output (GPIO_LEFT_GRND, GPIO.LOW)
        GPIO.output (GPIO_RIGHT_GRND, GPIO.HIGH)
        self.PWM_LEFT.ChangeDutyCycle( self.speed)
        self.PWM_RIGHT.ChangeDutyCycle( self.speed)

    def right(self):
        GPIO.output (GPIO_LEFT_V, GPIO.LOW)
        GPIO.output (GPIO_RIGHT_V, GPIO.HIGH)
        GPIO.output (GPIO_LEFT_GRND, GPIO.HIGH)
        GPIO.output (GPIO_RIGHT_GRND, GPIO.LOW)
        self.PWM_LEFT.ChangeDutyCycle( self.speed)
        self.PWM_RIGHT.ChangeDutyCycle( self.speed)

    def curverightforward(self): # CURVERIGHTFORWARD
        GPIO.output(GPIO_LEFT_V, GPIO.HIGH)
        GPIO.output(GPIO_RIGHT_V, GPIO.HIGH)
        GPIO.output(GPIO_LEFT_GRND, GPIO.LOW)
        GPIO.output(GPIO_RIGHT_GRND, GPIO.LOW)
        self.PWM_LEFT.ChangeDutyCycle( self.speed-self.diffForCurve)
        self.PWM_RIGHT.ChangeDutyCycle( self.speed+self.diffForCurve)

    def curveleftforward(self): # CURVELEFTFORWARD
        GPIO.output(GPIO_LEFT_V, GPIO.HIGH)
        GPIO.output(GPIO_RIGHT_V, GPIO.HIGH)
        GPIO.output(GPIO_LEFT_GRND, GPIO.LOW)
        GPIO.output(GPIO_RIGHT_GRND, GPIO.LOW)
        self.PWM_LEFT.ChangeDutyCycle( self.speed+self.diffForCurve)
        self.PWM_RIGHT.ChangeDutyCycle( self.speed-self.diffForCurve)

    def curverighbackward(self): # CURVERIGHBACKWARD
        GPIO.output (GPIO_LEFT_V, GPIO.LOW)
        GPIO.output (GPIO_RIGHT_V, GPIO.LOW)
        GPIO.output (GPIO_LEFT_GRND, GPIO.HIGH)
        GPIO.output (GPIO_RIGHT_GRND, GPIO.HIGH)
        self.PWM_LEFT.ChangeDutyCycle( self.speed-self.diffForCurve)
        self.PWM_RIGHT.ChangeDutyCycle( self.speed+self.diffForCurve)

    def curveleftbackward(self): # CURVELEFTBACKWARD
        GPIO.output (GPIO_LEFT_V, GPIO.LOW)
        GPIO.output (GPIO_RIGHT_V, GPIO.LOW)
        GPIO.output (GPIO_LEFT_GRND, GPIO.HIGH)
        GPIO.output (GPIO_RIGHT_GRND, GPIO.HIGH)
        self.PWM_LEFT.ChangeDutyCycle( self.speed+self.diffForCurve)
        self.PWM_RIGHT.ChangeDutyCycle( self.speed-self.diffForCurve)

Damit die Motoren auch über den Websocket die Befehle erhalten können, muss nun noch ein Websocket Client für die Motorsteuerung geschrieben werden. Dieser sendet wie jeder von uns geschriebene Client zunächst seinen Namen zum Websocket Server und kann dann Nachrichten empfangen oder senden. In diesem Fall muss der Client nur die Befehle Empfangen können.

import asyncio
import websockets
import DC_Motor

async def wsclient():
	async with websockets.connect('ws://localhost:9000') as websocket:
		motor = DC_Motor.DC_Motor()
		await websocket.send('Name:'+'motorControllWS')
		while True:
			msg = await websocket.recv()
			values = msg.split(':')
			motor.setMotorOrder(values[0],values[1])
			#msg = await websocket.send('MotorControllSet'+msg)
			#print(values)

#This have to be the last line!
asyncio.get_event_loop().run_until_complete(wsclient())