手柄控制jetson nano 2gb舵机转向c6智能小车

在jupyterlab上实现手柄控制舵机转向小车

shoubing.ipynb内容如下：

1.用游戏手柄控制机器人

想要实现手柄遥控，我们要做的第一件事是创建 Controller widget 的实例，我们将用它来驱动机器人。该 Controller widget 需要一个索引参数，该参数指定控制器的数量。如果您连接了多个控制器，或者某些游戏手柄显示为多个控制器，这很有用。要确定您正在使用的控制器的索引。

1.访问 http://html5gamepad.com

2.将手柄连接到PC电脑，并在您使用的游戏手柄上按下按钮

3.记住响应按钮按下的游戏手柄索引 index 的编号

接下来，我们将使用该索引创建并显示我们的控制器。

import ipywidgets.widgets as widgets

controller = widgets.Controller(index=0)  # 替换为你的手柄的 index 编号

display(controller)

注意事项： 即使索引正确，您也可能会看到文本 Connect gamepad and press any button。这是因为游戏手柄尚未在此 notebook 上注册。按下一个按钮，您应该看到游戏手柄 widget 出现在上方

2.将游戏手柄控制器连接到机器人马达

现在，即使我们已经连接了游戏手柄，也尚未将控件附加到机器人上！我们要附加的第一个也是最简单的控件是电动机控件。我们将使用 dlink 函数将其连接到左右垂直轴。与 link 函数不同，该 dlink 函数允许我们在 source 和之间附加一个转换 target。由于控制器轴偏离了我们认为对电机控制直观的位置，因此我们将使用一个小的 lambda 函数来取反该值。

警告：如果您触摸游戏手柄控制器的轴，那么下一个单元将移动机器人！

from zl_car import Car
import traitlets
import servo_control

car = Car()

motor_link = traitlets.dlink((controller.axes[1], 'value'), (car.motor, 'value'), transform=lambda x: -x)   #row 0
servo_link = traitlets.dlink((controller.axes[5], 'value'), (car.servo, 'value'), transform=lambda x: -x)   #col 2

接下来，我们再摇动手柄的方向杆，小车就会根据手柄的指令做出对应的反应了。

按键控制云台转动

def camera_up(change):
    if change['new']:
        car.camera_up_down(-500)

def camera_down(change):
    if change['new']:
        car.camera_up_down()

def camera_left(change):
    if change['new']:
        car.camera_l_r()

def camera_right(change):
    if change['new']:
        car.camera_l_r(-500)

controller.buttons[4].observe(camera_up, names='value')  #camera up
controller.buttons[6].observe(camera_down, names='value')  #camera down
controller.buttons[5].observe(camera_left, names='value')  #camera left
controller.buttons[7].observe(camera_right, names='value')  #camera right

 

按键控制OLED显示

import OLED

def oled(change):
    if change['new']:
        OLED.oled_main()
    else:
        OLED.oled_clear()

controller.buttons[2].observe(oled, names='value')

按键控制有源蜂鸣器

import beep

def button_beep(change):
    try:
        if change['new']:
            beep.on()
        else:
            beep.off()
    except KeyboardInterrupt:
        beep.destroy()
            
controller.buttons[1].observe(button_beep, names='value')

3.采集摄像头图像并显示

需要显示图像，我们需要导入Camera类库

首先，让我们显示一个widget的 Image 方法 ，用它来显示实时摄像机的显示窗口。我们将 height 和 width 设置为 300 像素，这样就不会占用太多空间。

image = widgets.Image(format='jpeg', width=300, height=300)

display(image)

4.创建相机实例

现在，目前没有图像，因为我们还没有设置值！我们可以通过创建我们的 Camera 类并将相机的 value 属性附加到图像的 value 属性

from jetcam.usb_camera import USBCamera

camera = USBCamera(capture_device=0,width=300, height=300)
camera.running=True
'''
from jetbot import Camera
camera = Camera.instance(width=300, height=300)
'''
print("camera created")

5.将相机连接到 widget图像控件

我们的相机类当前仅产生 BGR8（蓝色，绿色，红色，8位）格式的值，而我们的图像 widget 则接受压缩的 JPEG 值。要将相机连接到图像，我们需要在链接中插入 bgr8_to_jpeg 函数作为转换。代码如下进行转换

from jetbot import bgr8_to_jpeg

camera_link = traitlets.dlink((camera, 'value'), (image, 'value'), transform=bgr8_to_jpeg)

现在，您应该看到上面显示的实时视频！

提醒：您可以右键单击单元格的输出，然后选择 Create New View for Output 在单独的窗口中显示该单元格

用heartbeat处理断网异常

用户可以用手柄控制Jetbot的运动，同时也可以看到图像，这意味着我们可以不用看实际的Jetbot实物，而只需要通过图像中的摄像头采集的图片就可以控制Jetbot的运动了。

您可以通过浏览视频源来驱动机器人。但是，如果您的机器人与 Wifi 断开连接该怎么办？好吧，电动机将继续运转，并且将继续尝试传输视频和电机命令。让我们做到这一点，以便在断开连接时停止机器人并断开相机和电机的链接。

from jetbot import Heartbeat


def handle_heartbeat_status(change):
    if change['new'] == Heartbeat.Status.dead:
        camera_link.unlink()
        motor_link.unlink()
        servo_link.unlink()
        car.stop()

heartbeat = Heartbeat(period=0.5)

# 将回调函数附加到心跳状态
heartbeat.observe(handle_heartbeat_status, names='status')

如果机器人断开互联网连接，您会注意到它停止了。然后，您可以通过与下面的单元格重新创建链接来重新连接摄像头和电机

# 仅当您的机器人链接未链接时才调用此链接，否则我们将拥有多余的链接，该链接将增加一倍传输的命令

motor_link = traitlets.dlink((controller.axes[1], 'value'), (car.motor, 'value'), transform=lambda x: -x)
servo_link = traitlets.dlink((controller.axes[0], 'value'), (car.servo, 'value'), transform=lambda x: -x)
camera_link = traitlets.dlink((camera, 'value'), (image, 'value'), transform=bgr8_to_jpeg)

6.使用游戏手柄按钮保存快照

现在，我们希望能够从机器人中保存一些图像。让我们做一下，以便右缓冲挡（index 5）保存当前实时图像的快照。我们会将图像保存在 snapshots/ 目录中，并使用 uuid python package(包) 确保其名称唯一。我们使用 uuid1 标识符，因为它还会对日期和 MAC 地址进行编码，以备日后使用。

import uuid
import subprocess

subprocess.call(['mkdir', '-p', 'snapshots'])

snapshot_image = widgets.Image(format='jpeg', width=300, height=300)

def save_snapshot(change):
    # 当按键按下保存快照
    if change['new']:
        file_path = 'snapshots/' + str(uuid.uuid1()) + '.jpg'
        
        # 把快照写到文件 (我们使用图像值代替摄像头，因为它已经是 JPEG 格式了)
        with open(file_path, 'wb') as f:
            f.write(image.value)
            
        # 显示已经保存的快照
        snapshot_image.value = image.value


controller.buttons[3].observe(save_snapshot, names='value')

display(widgets.HBox([image, snapshot_image]))
display(controller)

 

#!/bin/bash

sudo kill -9 `ps -elf|grep mjpg |awk '{print $4}'|awk 'NR==1'`

sudo kill -9 `ps -elf|grep mjpg |awk '{print $4}'|awk 'NR==1'`

import atexit
from Adafruit_MotorHAT import Adafruit_MotorHAT
import traitlets
from traitlets.config.configurable import Configurable


class Motor(Configurable):

    value = traitlets.Float()
    
    # config
    alpha = traitlets.Float(default_value=1.0).tag(config=True)
    beta = traitlets.Float(default_value=0.0).tag(config=True)

    def __init__(self, driver, channel, *args, **kwargs):
        super(Motor, self).__init__(*args, **kwargs)  # initializes traitlets

        self._driver = driver
        self._motor = self._driver.getMotor(channel)
        if(channel == 1):
            self._ina = 1
            self._inb = 0
        else:
            self._ina = 2
            self._inb = 3
        atexit.register(self._release)
        
    @traitlets.observe('value')
    def _observe_value(self, change):
        self._write_value(change['new'])

    def _write_value(self, value):
        """Sets motor value between [-1, 1]"""
        mapped_value = int(255.0 * (self.alpha * value + self.beta))
        speed = min(max(abs(mapped_value), 0), 255)
        self._motor.setSpeed(speed)
        if mapped_value < 0:
            self._motor.run(Adafruit_MotorHAT.FORWARD)
            # The two lines below are required for the Waveshare JetBot Board only
            self._driver._pwm.setPWM(self._ina,0,0)
            self._driver._pwm.setPWM(self._inb,0,speed*16)
        else:
            self._motor.run(Adafruit_MotorHAT.BACKWARD)
            # The two lines below are required for the Waveshare JetBot Board only
            self._driver._pwm.setPWM(self._ina,0,speed*16)
            self._driver._pwm.setPWM(self._inb,0,0)

    def _release(self):
        """Stops motor by releasing control"""
        self._motor.run(Adafruit_MotorHAT.RELEASE)
        # The two lines below are required for the Waveshare JetBot Board only
        self._driver._pwm.setPWM(self._ina,0,0)
        self._driver._pwm.setPWM(self._inb,0,0)

#导包
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)
#print('end')

import time

import Adafruit_SSD1306

from PIL import Image
from PIL import ImageDraw
from PIL import ImageFont

# 128x32显示器，硬件I2C:
disp = Adafruit_SSD1306.SSD1306_128_32(rst=None, i2c_bus=1, gpio=1)

# 初始化库。
disp.begin()
# 清除显示内容
disp.clear()
disp.display()

#为绘图创建空白图像。
#确保为1位颜色创建模式为“1”的图像（单色）
width = disp.width
height = disp.height
image = Image.new('1', (width, height))

# 载入默认字体
font = ImageFont.load_default()

# 获取要在图像上绘制的绘图对象。
draw = ImageDraw.Draw(image)

def oled_main():
    # 画一个黑色填充框清除图像。
    draw.rectangle((0,0,width,height), outline=0, fill=0)
    draw.text((50,10),"ZLTech",font=font,fill=255)

    # 显示图像。
    disp.image(image)
    disp.display()

def oled_clear():
    disp.clear()
    disp.display()
        
if __name__ == '__main__':
    try:
        oled_main()
    except KeyboardInterrupt:
        oled_clear()

import time
import RPi.GPIO as GPIO

pin = 6
GPIO.setmode(GPIO.BCM)
GPIO.setwarnings(False)

GPIO.setup(pin, GPIO.OUT, initial=0)

def on():
    GPIO.output(pin, 1)
    
def off():
    GPIO.output(pin, 0)

def loop(x):
    on()
    time.sleep(x)
    off()
    time.sleep(x)
    
def destroy():
    off()
    GPIO.cleanup()
    
if __name__ == '__main__': 
    try:  # 检测异常
        loop(0.5)      # 调用循环函数
    except KeyboardInterrupt:  # 当按下Ctrl+C时，将执行destroy()子程序
        destroy()    # 释放资源

import time
import z_uart as myUart
from threading import Thread
import traitlets
from traitlets.config.configurable import SingletonConfigurable
from Adafruit_MotorHAT import Adafruit_MotorHAT
from motor import Motor
import Adafruit_PCA9685


systick_ms_bak = 0
to=''
tn=''

class Car(SingletonConfigurable):
    motor = traitlets.Instance(Motor)
    servo = traitlets.Instance(Motor)

    # config
    i2c_bus = traitlets.Integer(default_value=1).tag(config=True)
    motor_channel = traitlets.Integer(default_value=1).tag(config=True)
    motor_alpha = traitlets.Float(default_value=1.0).tag(config=True)
    servo_channel = traitlets.Integer(default_value=2).tag(config=True)
    servo_alpha = traitlets.Float(default_value=1.0).tag(config=True)
    '''
    #摇杆控制云台舵机
    yuntai_channel1 = traitlets.Integer(default_value=3).tag(config=True)
    yuntai_alpha1 = traitlets.Float(default_value=1.0).tag(config=True)
    yuntai_channel2 = traitlets.Integer(default_value=4).tag(config=True)
    yuntai_alpha2 = traitlets.Float(default_value=1.0).tag(config=True)
    '''
    #yuntai_channel = traitlets.Integer(default_value=3).tag(config=True)
    #yuntai_alpha = traitlets.Float(default_value=0.0).tag(config=True)
    
    def __init__(self, left_speed=0, right_speed=0, pwm_value=1500, flag=False, *args, **kwargs):
        #self.left_speed = left_speed
        #self.right_speed = right_speed
        self.motor_driver = Adafruit_MotorHAT(i2c_bus=self.i2c_bus)
        self.motor = Motor(self.motor_driver, channel=self.motor_channel, alpha=self.motor_alpha)
        self.servo = Motor(self.motor_driver, channel=self.servo_channel, alpha=self.servo_alpha)
        '''
        #摇杆控制云台舵机
        self.yuntai_servo1 = Motor(self.motor_driver, channel=self.yuntai_channel1, alpha=self.yuntai_alpha1)
        self.yuntai_servo2 = Motor(self.motor_driver, channel=self.yuntai_channel2, alpha=self.yuntai_alpha2)
        '''
        #self.yuntai_servo = Motor(self.motor_driver, channel=self.yuntai_channel, alpha=self.yuntai_alpha)
        #self.pwm_value = pwm_value
        self.flag = flag
        self.uart = myUart
        self.uart.setup_uart(115200)
        self.pwm = Adafruit_PCA9685.PCA9685()
        self.pwm.set_pwm_freq(50)
        self.pan = 90
        self.tilt = 90
        t = Thread(target=self.car_move,)
        t.setDaemon(True)
        t.start()
    
    #摇杆控制云台舵机
    def servo_move(self):
        self.pwm.set_pwm(0,0,int(4096*(((float(str(self.yuntai_servo1.value))*90+90)*11)+500)/20000))
        self.pwm.set_pwm(1,0,int(4096*(((90-float(str(self.yuntai_servo2.value))*90)*11)+500)/20000))
    
    def servo_control(self, basechannel,basevalue,tiltchannel,tiltvalue):
        basevalue=4096*((basevalue*11)+500)/20000
        self.pwm.set_pwm(basechannel,0,int(basevalue))
        tiltvalue=4096*((tiltvalue*11)+500)/20000
        self.pwm.set_pwm(tiltchannel,0,int(tiltvalue))
    
    #摇杆按键控制摄像头转动，不太管用
    def button_servo_move(self):
        #print(self.yuntai_servo.value)
        if float(str(self.yuntai_servo.value)) < -0.5:
            self.camera_up_down(-500)
        elif 0 < float(str(self.yuntai_servo.value)) < 0.2:
            self.camera_up_down()
        elif 0.5 < float(str(self.yuntai_servo.value)) < 1:
            self.camera_l_r()
        elif -0.5 < float(str(self.yuntai_servo.value)) < 0:
            self.camera_l_r(-500)
    
    #摄像头up down转动    
    def camera_up_down(self, speed=500):
        global systick_ms_bak
        if(int((time.time() * 1000))- systick_ms_bak > 20):
            systick_ms_bak = int((time.time() * 1000))
            self.tilt = self.tilt + speed//100
            if self.tilt > 180:
                self.tilt = 180
            if self.tilt < 0:
                self.tilt = 0
            self.servo_control(0, self.pan, 1, self.tilt)

    #摄像left right转动    
    def camera_l_r(self, speed=500):
        global systick_ms_bak
        if(int((time.time() * 1000))- systick_ms_bak > 20):
            systick_ms_bak = int((time.time() * 1000))
            self.pan = self.pan + speed//100
            if self.pan > 180:
                self.pan = 180
            if self.pan < 0:
                self.pan = 0
            self.servo_control(0, self.pan, 1, self.tilt)
    
    
    def car_move(self):
        try:
            while 1:
                self.car_run()
                #self.servo_move()
                #self.button_servo_move()  #摇杆按键控制摄像头转动，不太管用
                time.sleep(0.01)
        except:
            self.stop()
        finally:
            self.stop()

    def car_run(self):
        global to,tn
        self.motor_speed = float(str(self.motor.value)) * 500
        self.servo_speed = 1500 - float(str(self.servo.value))*1000
        if abs(self.motor_speed) < 4:
            self.motor_speed = 0
        if 1450 < abs(self.servo_speed) < 1550:
            self.servo_speed = 1500
        if self.flag:
            self.motor_speed = 0
            self.servo_speed = 1500
        testStr = "{#006P%04dT0000!#007P%04dT0000!#000P%04dT0000!}" % (1500+self.motor_speed, 1500-self.motor_speed, self.servo_speed)
        tn = testStr
        if to != tn:
            print(testStr)
            to = tn
            
        self.uart.uart_send_str(testStr)

    def stop(self):
        self.flag = True
        

 

注：zl_car代码根据jetbot源码改编

　　analog 模式切换

       motor.Motor类型转为float， 

　　编码报错解决办法（z_uart文件中的解码格式用encoding="unicode_escape"）：uart_receive_buf = uart_receive_buf+ser.read(uart_receive_buf_index).decode(encoding="unicode_escape")