自動跟隨機器人教程(四)軟體部分 樹莓派+電腦 控制小車移動
阿新 • • 發佈:2019-01-08
接下來可以說是本教程的核心內容了,不可能在一篇文章中講完,首先講一個比較初級的程式。樹莓派可以接收同一個區域網上電腦的連線,並且電腦鍵盤上發出的前後左右的控制訊號通過網路發向樹莓派後,樹莓派再經過串列埠發向Arduino,對電機進行控制。
樹莓派需要實現的程式分為兩部分,一個是Socket伺服器,另一個是串列埠寫入的程式碼。涉及到多執行緒知識。
客戶端程式作為Socket客戶端連入樹莓派,然後通過pygame庫獲取鍵盤命令後把運動指令發往樹莓派。
樹莓派的程式稍微複雜些,首先主迴圈是監聽客戶端連入,連入後用另一個執行緒對從客戶端發來的資料進行處理,把指令用串列埠下發給Arduino。當指令涉及到超聲波讀取時,樹莓派用另一個執行緒去監聽串列埠收到的資料,這個資料就是距離資料(為了儘量簡化,下面的程式碼範例中不含超聲波讀取部分,完整部分參見後面教程中的最終程式)。
下面貼上部分程式碼:
首先是運行於小車樹莓派中的程式:
#!/usr/bin/env python import socket import sys import threading import random import os import time import struct import serial #import cv #import Image,StringIO port_serial="/dev/ttyACM0" sl = serial.Serial(port_serial,9600) HOST = "0.0.0.0" PORT = 9004 SOCK_ADDR = (HOST, PORT) exit_now = 0 def exit_signal_handle(sig,stack_frame): global exit_now print "EXIT sig" exit_now = 1 class serial_thread(threading.Thread): def __init__(self): threading.Thread.__init__(self) def run(self): self.running = True while self.running: try: data=sl.readline() print data except: print sys.exc_info() def stop(self): self.running = False def forward(): print "forward" string="1" sl.write(string) def reverse(): print "reverse" string="2" sl.write(string) def pivot_left(): print "left" string="3" sl.write(string) def pivot_right(): print "right" string="4" sl.write(string) def pause(): print "pause" string="0" sl.write(string) def net_input(command): global laser_index_vertical global laser_index_horizontal if command == 1: forward() elif command == 2: reverse() elif command == 3: pivot_left() elif command == 4: pivot_right() elif command == 0: pause() ### class SocketClientObject(object): def __init__(self, socket, address ): self.socket = socket self.address = address ### class ClientThread(threading.Thread): def __init__(self, client_object): threading.Thread.__init__(self) self.client_object = client_object def run(self): self.running = True while self.running: data = self.client_object.socket.recv(1024) print ">> Received data: ", data, " from: ", self.client_object.address if(data=='0'): net_input(0) elif(data=='1'): net_input(1) elif(data=='2'): net_input(2) elif(data=='3'): net_input(3) elif(data=='4'): net_input(4) elif(data=='5'): break print "client_quit" self.client_object.socket.close() def stop(self): self.running = False ### def main(): ser_th = serial_thread() ser_th.start() try: sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM) sock.bind(SOCK_ADDR) sock.listen(5) while exit_now == 0: # accept connections from outside (clientsocket, address) = sock.accept() print " Accept client: ", address # now do something with the clientsocket # in this case, we'll pretend this is a threaded server ct = ClientThread(SocketClientObject(clientsocket, address)) ct.start() except: print "#! EXC: ", sys.exc_info() #sock.close() #ser_th.stop() #ser_th.join() print "THE END! Goodbye!" ### if __name__ == "__main__": main()
接下來是電腦控制端執行的程式:
#!/usr/bin/env python import socket import time import pygame import cv2.cv as cv import Image, StringIO import threading import cv2 import numpy as np def main(): sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM) sock.connect(('192.168.1.10',9004)) pygame.init() W, H = 320, 240 screen = pygame.display.set_mode((W, H)) clock = pygame.time.Clock() running = True command_to_send=0 command_last=0 laser_control = False while running: command_last=command_to_send for event in pygame.event.get(): if event.type == pygame.QUIT: command_to_send=5 running = False elif event.type == pygame.KEYUP: command_to_send=0 elif event.type == pygame.KEYDOWN: if event.key == pygame.K_UP: command_to_send=1 elif event.key == pygame.K_DOWN: command_to_send=2 elif event.key == pygame.K_LEFT: command_to_send=3 elif event.key == pygame.K_RIGHT: command_to_send=4 if(command_to_send!=command_last): sock.send(str(command_to_send)) clock.tick(50) sock.close() if __name__ == '__main__': main()
最後是小車arduino中執行的程式碼(只包含控制部分,不含超聲波):
int led = 13;
void setup()
{
Serial.begin(9600);
pinMode(led, OUTPUT);
pinMode(9,OUTPUT);
pinMode(10,OUTPUT);
pinMode(5,OUTPUT);
pinMode(6,OUTPUT);
}
void loop()
{
//digitalWrite(led, HIGH); // turn the LED on (HIGH is the voltage level)
delay(100); // wait for a second
//digitalWrite(led, LOW); // turn the LED off by making the voltage LOW
//delay(1000); // wait for a second
if (Serial.available()>0)
{
char cmd = Serial.read();
Serial.print(cmd);
switch (cmd)
{
case '1':
Serial.println("Forward");
Forward();
break;
case '2':
Serial.println("Back");
Back();
break;
case '3':
Serial.println("Left");
Turn_left();
break;
case '4':
Serial.println("Right");
Turn_right();
break;
default:
Stop();
}
}
}
void Forward()
{
digitalWrite(9,HIGH);
digitalWrite(10,LOW);
digitalWrite(5,HIGH);
digitalWrite(6,LOW);
}
void Back()
{
digitalWrite(9,LOW);
digitalWrite(10,HIGH);
digitalWrite(5,LOW);
digitalWrite(6,HIGH);
}
void Turn_right()
{
digitalWrite(9,LOW);
digitalWrite(10,HIGH);
digitalWrite(5,HIGH);
digitalWrite(6,LOW);
}
void Turn_left()
{
digitalWrite(9,HIGH);
digitalWrite(10,LOW);
digitalWrite(5,LOW);
digitalWrite(6,HIGH);
}
void Stop()
{
digitalWrite(9,LOW);
digitalWrite(10,LOW);
digitalWrite(5,LOW);
digitalWrite(6,LOW);
}