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! :)