1. 程式人生 > >自動跟隨機器人教程(四)軟體部分 樹莓派+電腦 控制小車移動

自動跟隨機器人教程(四)軟體部分 樹莓派+電腦 控制小車移動

接下來可以說是本教程的核心內容了,不可能在一篇文章中講完,首先講一個比較初級的程式。樹莓派可以接收同一個區域網上電腦的連線,並且電腦鍵盤上發出的前後左右的控制訊號通過網路發向樹莓派後,樹莓派再經過串列埠發向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);
}