手机app控制jetson nano 2gb舵机转向c6智能小车运动及云台运动

手机app控制jetson nano 2gb舵机转向c6智能小车前进、后退、左转、右转；云台摄像头向上、向下、向左、向右

# -*- coding:utf-8 -*-
#导入模块
import time
import threading
import z_uart as myUart
import z_socket as mySocket
import servo_control

#全局变量定义
systick_ms_bak = 0
car_mode = 0
car_mode_bak = 0
pan = 90
tilt = 90


#串口检测
def loop_uart():
    if myUart.uart_get_ok == 2:
        print(myUart.uart_receive_buf)
        parse_cmd(myUart.uart_receive_buf)
        myUart.uart_receive_buf = ''
        myUart.uart_get_ok = 0
    elif myUart.uart_get_ok == 1 or myUart.uart_get_ok == 3:
        print(myUart.uart_receive_buf)
        myUart.uart_send_str(myUart.uart_receive_buf)
        myUart.uart_receive_buf = ''
        myUart.uart_get_ok = 0
#网络连接服务
def loop_socket():
    if mySocket.socket_get_ok:
       myUart.uart_get_ok = mySocket.socket_get_ok
       myUart.uart_receive_buf = mySocket.socket_receive_buf
       mySocket.socket_receive_buf = ''
       mySocket.socket_get_ok = 0
#指令解析       
def parse_cmd(myStr):
    global car_mode,car_mode_bak,voice_flag,voice_mode
    if myStr.find('$QJ!') >= 0:
        car_mode = 1
    elif myStr.find('$HT!') >= 0:
        car_mode = 2
    elif myStr.find('$ZZ!') >= 0:
        car_mode = 3
    elif myStr.find('$YZ!') >= 0:
        car_mode = 4
    elif myStr.find('$TZ!') >= 0:
        car_mode = 0
    elif myStr.find('$CUP!') >= 0:
        car_mode = 9
    elif myStr.find('$CDOWN') >= 0:
        car_mode = 10
    elif myStr.find('$CLEFT') >= 0:
        car_mode = 11
    elif myStr.find('$CRIGHT') >= 0:
        car_mode = 12

def loop_car_mode():
    global car_mode,car_mode_bak
    if car_mode_bak != car_mode:
        car_mode_bak = car_mode
        if car_mode == 0:  # 停车
            #myUart.uart_send_str("#012P1515T1000!")
            car_run(0)
        elif car_mode == 1:  #前进
            #myUart.uart_send_str("#012P1511T1000!")
            car_run(250)
        elif car_mode == 2:  #后退
            #myUart.uart_send_str("#012P1512T1000!")
            car_run(-250)
        elif car_mode == 3:  #left转
            #myUart.uart_send_str("#012P1513T1000!")
            car_run(250, 900)
        elif car_mode == 4:  #right转
            #myUart.uart_send_str("#012P1514T1000!")
            car_run(250, 2200)

    elif car_mode == 9:  #up
        camera_up_down(-100)
    elif car_mode == 10:  #down
        camera_up_down(100)
    elif car_mode == 11:  #left
        camera_l_r(100)
    elif car_mode == 12:  #right
        camera_l_r(-100)  

'''
函数功能：串口发送指令控制电机转动
范围：-1000～+1000
'''

def car_run(speed, pwm_value=1500):
    testStr = "{#006P%04dT0000!#007P%04dT0000!#000P%04dT0000!}" % (1500+speed, 1500-speed, pwm_value)
    print(testStr)
    myUart.uart_send_str(testStr)


#摄像头up down转动    
def camera_up_down(speed):
    global systick_ms_bak,tilt,pan
    if(int((time.time() * 1000))- systick_ms_bak > 20):
        systick_ms_bak = int((time.time() * 1000))
        tilt = tilt +speed//100
        if tilt > 180:
            tilt = 180
        if tilt < 0:
            tilt = 0
        servo_control.servo_move(0, pan, 1, tilt)

#摄像left right转动    
def camera_l_r(speed):
    global systick_ms_bak,pan,tilt
    if(int((time.time() * 1000))- systick_ms_bak > 20):
        systick_ms_bak = int((time.time() * 1000))
        pan = pan + speed//100
        if pan > 180:
            pan = 180
        if pan < 0:
            pan = 0
        servo_control.servo_move(0, pan, 1, tilt)


#大循环
if __name__ == '__main__':
    time.sleep(5)
    myUart.setup_uart(115200) #设置串口
    
    mySocket.setup_socket(1314)
    servo_control.servo_move(0, pan, 1, tilt)
   
    try:
        while True:
            loop_uart()
            loop_socket()
            loop_car_mode()
    except KeyboardInterrupt:
        exit()

import threading
import socket

socket_port = 1314
socket_get_ok = 0
socket_receive_buf = ""

def socketEvent():
    global socket_receive_buf,socket_get_ok, socket_port
    serverSocket = socket.socket()
    #ip addr cannot conver to str!!! TODO 
    #hostIP = str(os.popen('ifconfig wlan0 | grep "inet " | awk \'{print $2}\'').read())
    hostIP = "192.168.12.1"
    print('hostIP:', hostIP)
    #data port
    dataPort = socket_port
    address = (hostIP,dataPort)
    serverSocket.bind(address)
    
    serverSocket.listen(5)
    print('socket waitting......')
    serverConnect,addr = serverSocket.accept()
    print('socket connected!')

    mode = 0
    while True:
        data = str(serverConnect.recv(1024))
        data = data.split('\'')[1]
        if len(data):
            print(data)
            socket_receive_buf = data
            if socket_get_ok == 0:
                uart_receive_buf = socket_receive_buf
                uart_get_ok = 0
                if mode == 0:
                    if uart_receive_buf.find('{') >= 0:
                        mode = 1
                        #print('mode1 start')
                    elif uart_receive_buf.find('$') >= 0:
                        mode = 2
                        #print('mode2 start')
                    elif uart_receive_buf.find('#') >= 0:
                        mode = 3
                        #print('mode3 start')
                
                if mode == 1:
                    if uart_receive_buf.find('}') >= 0:
                        uart_get_ok = 1
                        mode = 0
                        #print('{}:',uart_receive_buf, " len:", len(uart_receive_buf))
                        #print('mode1 end')
                elif mode == 2:
                    if uart_receive_buf.find('!') >= 0:
                        uart_get_ok = 2
                        mode = 0
                        #print('$!:',uart_receive_buf, " len:", len(uart_receive_buf))
                        #print('mode2 end')
                elif mode == 3:
                    if uart_receive_buf.find('!') >= 0:
                        uart_get_ok = 3
                        mode = 0
                        #print('#!:', uart_receive_buf, " len:", len(uart_receive_buf))
                        #print('mode3 end')
                socket_get_ok = uart_get_ok
                #print('get2:',uart_receive_buf, " len:", len(uart_receive_buf), " mode:", mode, " getok:", uart_get_ok)
    
        else:
            print('socket disconnected.')
            serverSocket.listen(5)
            print('socket waitting......')
            serverConnect,addr = serverSocket.accept()
            print('socket connected!')
            
socket_thread = threading.Thread(target=socketEvent)

def setup_socket(port):
    global socket_thread, socket_port
    socket_port = port
    #socket接收线程
    socket_thread.start()

def loop_socket():
    global socket_get_ok, socket_receive_buf
    if socket_get_ok:
        print(socket_receive_buf)
        socket_get_ok = 0

#大循环
if __name__ == '__main__':
    setup_socket(2020)
    try:
        while True:
            loop_socket()
            
    except KeyboardInterrupt:
        if serverSocket != None:
            serverSocket.close()

#导包
import serial
import time
import threading

#全局变量定义
ser = ''
uart_baud = 115200
uart_get_ok = 0
uart_receive_buf = ""
uart_receive_buf_index = 0

#发送字符串 只需传入要发送的字符串即可
def uart_send_str(string):
    global ser
    ser.write(string.encode("utf-8"))
    time.sleep(0.01)
    ser.flushInput()

#线程调用函数，主要处理数据接受格式，主要格式为 $...!   #...! {...}三种格式，...内容长度不限
def serialEvent():
    global ser,uart_receive_buf_index,uart_receive_buf,uart_get_ok
    mode = 0
    try:
        while True:
            if uart_get_ok == 0:
                uart_receive_buf_index = ser.inWaiting()
                if uart_receive_buf_index > 0:
                    uart_receive_buf = uart_receive_buf+ser.read(uart_receive_buf_index).decode()
                    #print('get1:',uart_receive_buf, " len:", len(uart_receive_buf), " mode:", mode)
                    if mode == 0:
                        if uart_receive_buf.find('{') >= 0:
                            mode = 1
                            #print('mode1 start')
                        elif uart_receive_buf.find('$') >= 0:
                            mode = 2
                            #print('mode2 start')
                        elif uart_receive_buf.find('#') >= 0:
                            mode = 3
                            #print('mode3 start')
                    
                    if mode == 1:
                        if uart_receive_buf.find('}') >= 0:
                            uart_get_ok = 1
                            mode = 0
                            ser.flushInput()
                            #print('{}:',uart_receive_buf, " len:", len(uart_receive_buf))
                            #print('mode1 end')
                    elif mode == 2:
                        if uart_receive_buf.find('!') >= 0:
                            uart_get_ok = 2
                            mode = 0
                            ser.flushInput()
                            #print('$!:',uart_receive_buf, " len:", len(uart_receive_buf))
                            #print('mode2 end')
                    elif mode == 3:
                        if uart_receive_buf.find('!') >= 0:
                            uart_get_ok = 3
                            mode = 0
                            ser.flushInput()
                            #print('#!:', uart_receive_buf, " len:", len(uart_receive_buf))
                            #print('mode3 end')
                    
                    #print('get2:',uart_receive_buf, " len:", len(uart_receive_buf), " mode:", mode, " getok:", uart_get_ok)
    
    except IOError:
        pass;

#串口接收线程
uart_thread = threading.Thread(target=serialEvent)

#串口初始化
def setup_uart(baud):
    global ser,uart_thread,uart_receive_buf
    uart_baud = baud
    ser = serial.Serial("/dev/ttyTHS1", uart_baud)
    ser.flushInput()
    uart_thread.start()
    uart_send_str("uart init ok!\r\n");
    uart_receive_buf = ''
        


#大循环
if __name__ == '__main__':
    setup_uart(115200)
    
    #uart_send_str("#000P1500T1000!");
    #uart_send_str("{#000P1500T1000!#001P1500T1000!}");

    try:
        while True:
            pass
  
    except KeyboardInterrupt:
        if ser != None:
            ser.close()

import time
import Adafruit_PCA9685

pwm = Adafruit_PCA9685.PCA9685()
pwm.set_pwm_freq(50)

def servo_move(basechannel,basevalue,tiltchannel,tiltvalue):
    basevalue=4096*((basevalue*11)+500)/20000
    pwm.set_pwm(basechannel,0,int(basevalue))
    tiltvalue=4096*((tiltvalue*11)+500)/20000
    pwm.set_pwm(tiltchannel,0,int(tiltvalue))

pan = 90
tilt = 90

servo_move(0, pan, 1, tilt)


if __name__ == '__main__':

    for angle in range(30, 120, 5):
        servo_move(0, angle, 1, angle)
        time.sleep(0.5)

    servo_move(0, pan, 1, tilt)

手机连接小车发出的wifi，然后打开app登录，配置按钮指令：

前进：按下发送指令为 $QJ!    松开发送指令为 $TZ!
后退：按下发送指令为 $HT!    松开发送指令为 $TZ!
左转：按下发送指令为 $ZZ!    松开发送指令为 $TZ!
右转：按下发送指令为 $YZ!    松开发送指令为 $TZ!

云台向上：按下发送指令为 $UP!    松开发送指令为 $TZ!
向下：按下发送指令为 $DOWN!    松开发送指令为 $TZ!
向左：按下发送指令为 $LEFT!    松开发送指令为 $TZ!
向右：按下发送指令为 $RIGHT!    松开发送指令为 $TZ!

注：app_control.py文件中，大循环里的 time.sleep(5)  #延时5秒，使开机自启后socket有时间建立连接，不然只能看到画面，不能控制小车

开机自启动、后台运行：
gnome-session-properties打开自启动管理界面，添加一条新的自启动命令：
    nohup python3 /home/dlinano/ZLTech-demo/app/app_control.py &

其中：nohup和&为使其后台运行