Robot na bazie Raspberry Pi #3

Tym razem pragnę podzielić się z Wami kodem w Pyhonie, dzięki któremu moja uprzednio zaprezentowana konstrukcja robota jeżdżącego, będzie odbierała rozkazy po sieci LAN i poruszała się tak, jak pragnie tego operator.
Wybrałem w tym celu język Python, ponieważ jest on od razu w Raspberry Pi dostępny, daje bardzo prostą możliwość zmiany stanu portów GPIO, oraz można w nim bardzo łatwo zaimplementować protokół komunikacji z naszym robotem.
Python w ogóle jest językiem bardzo prostym i w tym tkwi jego siła.

Jak już poprzednio zaznaczyłem, pojazd założenia ma poruszać się w obrębie sieci LAN. Do odbierania danych z domowego routera, robot wykorzysta wewnętrzne WIFI, które Raspberry PI w wersji 3 posiada w standardzie.Po odpowiednim skonfigurowaniu domowego routera, polegającemu na przekierowaniu ruchu na wybranym porcie na zadany adres lokalny, przydzielony robotowi, możliwe będzie sterowanie robotem także spoza domu, z dowolnego miejsca na świecie.

Obrazuje to poniższy rysunek:

Na potrzeby projektu, zrealizowałem kod, który jak na razie składa się łącznie z trzech klas w Pythonie.
Komunikacja z urządzeniem polega na wysłaniu do niego rozkazu, zgodnego z prostym protokołem znakowym, przy użyciu bezpołączeniowego protokołu UDP. Odbiór tego typu danych zrealizowany został w klasie UdpServer:

import socket
import sys,select
from Robot import *
class UdpServer:
    
    def __init__(self):
        print "udpserver waiting for commands"
        self.port = 80
        self.bufferSize = 1024
        self.s = socket.socket(socket.AF_INET,socket.SOCK_DGRAM)
        self.s.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
        self.s.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1)
        self.s.bind(('',self.port))
        self.robot = Robot()
    def serverStart(self):      
        while True:
            receivedCommand, address = self.s.recvfrom(8192)
            print self.robot.ParseCommand(receivedCommand)

W powyższym kodzie zrealizowany został prosty serwer UDP, nasłuchujący na porcie 80.
Jak na razie dla uproszczenia komunikacja bazuje na odbieraniu danych z adresu rozgłoszeniowego(broadcast) sieci lokalnej. Dodatkowo w konstruktorze klasy tworzony jest obiekt klasy Robot.py, w którym zrealizowane jest parsowanie odebranych w serwerze danych oraz sterowanie portami GPIO Raspberry Pi . Odbieranie danych i wywoływanie metody Parse w obiekcie klasy Robot.py zrealizowane są w wiecznej pętli, po wywołaniu metody serwera o nazwie serverStart.

Poniżej kod klasy Robot.py

import RPi.GPIO as GPIO
import time
from time import sleep
class Robot:

    #konstruktor
    def __init__(self): 
    
        print "robot start"     
        self.spinning=True
        self.stopped=False
        self.motorLeft =17 #gpio 17
        self.motorRight=23 #gpio 23
        self.rpmPulse=1
        GPIO.setmode(GPIO.BCM)
        #GPIO.setmode(GPIO.BOARD) numery pinow zamiast numer gpio 
        GPIO.setwarnings(False)
        GPIO.setup(2,GPIO.OUT) #silnik lewy start stop
        GPIO.setup(22,GPIO.OUT)#lewy direction
                GPIO.setup(4,GPIO.OUT) #SLEEP
        GPIO.setup(17,GPIO.OUT)# silnik prawy
        GPIO.setup(27,GPIO.OUT) #prawy direction
        GPIO.output(2,GPIO.HIGH) # lewy 
        GPIO.output(17,GPIO.LOW)# prawy
        GPIO.output(4,GPIO.HIGH)# LOW = sleep   
        GPIO.output(22,GPIO.LOW) #poczatkowy kierunek lewego
        GPIO.output(27,GPIO.HIGH)#poczatkowy kierunek prawego
            
    def ParseCommand(self,udpCommand):
        if udpCommand =='forward':
            self.forward()
            return "go forward"
        elif udpCommand=='back':
            self.back()
            return "go back"
        elif udpCommand=='left':
            self.left()
            return " turn left"
        elif udpCommand=='right':
            self.right()
            return "turn right"
        elif udpCommand =='stop':
            self.stop()
            return "motors stopped"
        elif udpCommand =='start':
            self.start()
            return "motors start"
        else:
            return "unknown command"
                                
    def left(self): #skret w lewo
        self.stop()
        GPIO.output(2,GPIO.LOW)#zatrzymanie silnika prawego
        GPIO.output(27,GPIO.LOW)#ustawienie kierunku silnika lewego
        GPIO.output(17,GPIO.HIGH) #uruchoienie silnika lewego
            
    def right(self):  #skret w prawo
        self.stop()
        GPIO.output(17,GPIO.LOW) #zatrzymanie silnika lewego
        GPIO.output(22,GPIO.LOW) #poczatkowy kierunek lewego
        GPIO.output(2,GPIO.HIGH) #uruchomienie silnika prawego
        
        
    def stop(self):
        GPIO.output(2,GPIO.LOW)
        GPIO.output(17,GPIO.LOW)
        
    def start(self):
        GPIO.output(2,GPIO.HIGH)
        GPIO.output(17,GPIO.HIGH)
    
    def forward(self):
        #self.stop()
        GPIO.output(22,GPIO.LOW)
        GPIO.output(27,GPIO.LOW)
        #self.start()
    
    def back(self):
        #self.stop()
        GPIO.output(22,GPIO.HIGH)
        GPIO.output(27,GPIO.HIGH)
        #self.start()

W konstruktorze klasy zdefiniowane są porty GPIO używane przez robota. I tak kolejno:
port 2 - start/stop silnika prawego
port 22 - kierunek obrotów silnika prawego
port 17 - start/stop silnika lewego
port 27 - kierunek obrotów silnika lewego
port 4 - stan niski na tym porcie zatrzymuje oba silniki a stan wysoki włącza je, umożliwiając ich sterowanie w zależności od stanu odpowiednich,w/w portów.

Ponadto definiujemy stany początkowe każdego z portów po uruchomieniu programu.Metoda ParseCommand klasy Robot przetwarza komendy UDP odebrane w serwerze, przekazane do niej jako argument. Są to proste rozkazy tekstowe:

• forward
• back
• left
• right
• stop
• start

Jeśli rozkaz odpowiada jednej z komend zdefiniowanych w metodzie, następuje wywołanie odpowiedniej metody zmiany kierunku, lub startu/zatrzymania pojazdu.

Aby całość zadziałała, trzeba jeszcze w metodzie main w skrypcie Main.py utworzyć obiekt klasy UdpServer, a nastepnie wywołać metodę serveStart tego obiektu w której działa wieczna pętla odbierająca dane UDP.

#!/usr/bin/env python

from Robot import *
from UdpServer import *

def main(args):

        
    udpServer = UdpServer()
    udpServer.serverStart()
    return 0


if __name__ == '__main__':

         import sys
         sys.exit(main(sys.argv))

Powyższy kod pisałem troszkę "na kolanie" ale robi co do niego należy. Dobrze jest skonfigurować w linuksie na RPI automatyczne uruchamianie go po starcie.
Na pewno program można napisać lepiej - jak wszystko ;) Na codzień nie koduję w Pythonie.
Co można jeszcze dodać? Pewnie dużo. Przydałoby się posklejać klasę klienta UDP dla robota. Z obiektu tej klasy można odsyłać statusy do aplikacji sterującej. Ponadto dodanie kamery także pociągnie za sobą modyfikację kodu urządzenia. Jak widać zabawy szykuje się jeszcze sporo. Póki co jednak skupiam się na zrobieniu podstawowej, działającej wersji robota. Kolejnym krokiem będzie napisanie klienta, umożliwiającego wysyłanie rozkazów do robota i sterowanie nim.

Mam jeszcze pytanie - w jaki sposób zaznaczać całe bloki tekstu w taki sposób, aby np cały podany przeze mnie kod był na szaro? Za diabła nie mogę tego poustawiać :)
Pozdrawiam wszystkich! :)