首先总框（图片很全，来自 博客47991812）：
目录
目录 3
1. 系统搭建 1
2.界面设计 8
3. 功能和代码 16
3.1. 智能小车的电机驱动 16
3.1.1. 功能简介 16
3.1.2. 操作截图 17
3.1.3. 代码说明 17
3.2. 摄像头的使用，以及舵机与摄像头平台的转动 24
3.2.1. 功能简介 24
3.2.2. 操作截图 25
3.2.3. 代码说明 29
3.3. 蓝牙通信控制驱动 40
3.3.1. 功能简介 40
3.3.2. 操作截图 40
3.3.3. 代码说明 41
3.4. 蓝牙通信控制舵机 57
3.4.1. 功能简介 57
3.4.2. 操作截图 57
3.4.3. 代码说明 58
3.5. USB摄像头捕获道路图片同时获取方向信息 74
3.5.1. 功能简介 74
3.5.2. 操作截图 75
3.5.3. 代码说明 75
3.6. 根据超声波传感器距离调整 92
3.6.1. 功能简介 92
3.6.2. 操作截图 93
3.6.3. 代码说明 96
3.7. edgeboardAI 小车驱动实现 118
3.7.1. 功能简介 118
3.7.2. 操作截图 118
3.7.3. 代码说明 120
3.8. 基于edgeboardAI和WOBOT超声波微调 131
3.8.1. 功能简介 131
3.8.2. 操作截图 132
3.8.3. 代码说明 132
3.9. edgeboardAI 小车展旗，放粮架主要代码 151
3.9.1. 功能简介 151
3.9.2. 操作截图 151
3.9.3. 代码说明 152
3.10. 基于多个舵机的抓机的实现，以及1，2两块电机板驱动对于上层AI的调用接口实现 155
3.10.1. 功能简介 155
3.10.2. 操作截图 155
3.10.3. 代码说明 155
1.系统搭建
针对树莓派平台的系统的制作、驱动程序安装、各种软件的安装和配置（Qt、SSH、VNC等），详细撰写，相当于一个指南，其他人按照这个指南，就可以一步一步在空白的树莓派上把所有环境安装配置好。
0.搭建树梅派Linux系统，基于SD卡安装Ubuntu系统
1.点击 Software
2.Raspberry PI OS
3.下拉 根据 系统 选择 为选择的 是第二个
4.安装拷贝 工具
拷贝成功，将SD卡放回树莓派接口，
接入PC显示器/LCD屏等测试
启动 并 初始化 树莓派系统，为下载编译等环境做准备
1.Linux安装ssh使用ssh pi@地址 或者 putty工具进行SSH连接
2.需要用到树莓派操作系统，并且安装编译环境.py以及gpio库模板sudo pip install rpi.gpio
sudo apt-get update
sudo apt-get install -y python3
sudo apt-get install -y libatlas-base-dev
sudo apt-get install -y libjasper-dev
sudo apt-get install -y libqtgui4
用vim编写并.py并用python test.py 测试环境
3.安装socat和minicom和hterm模拟程序的通讯。
sudo apt-get install socat
sudo apt-get install minicom
下载hterm源码编译，./hterm运行
4.安装python带的蓝牙通信模块
pip install pybluez
导入模板import bluetooth
5.安装pygame手柄控制(360无线)
sudo apt-get install joystick
ls /dev/input/
jstest /dev/input/js0
然后调试手柄通信，
或者c语言版控制
gcc Xbox_Joystick.c -o Xbox_Joystick
sudo ./Xbox_Joystick
6.树莓派GPIO库安装
1)编译环境（python）
先安装python
sudo apt-get install python-dev
然后pip安装依赖
pip install RPi.GPIO
pip install spidev
2)然后安装I2C和UART接口库
sudo apt install python-smbus
sudo apt install python-serial
7.c语言给python写接口
gcc -fPIC -shared func.c -o libfunc.so 编译成动态库
Python test.py 运行调用c接口的函数测试
8.树莓派 USB摄像头 网络监控环境
1.确认设备
lsusb -> ls /dev 找到video0(或者有类似的设备)
2.安装MJPG-Streamer
先安装cmake环境：
sudo apt-get install cmake libjpeg8-dev
让后 拷贝源码
git clone ?https://e.coding.net/fivecc/mjpg-streamer/mjpg-streamer.git
或者
wget https://github.com/Five-great/mjpg-streamer/archive/master.zip
然后进入有cmake的目录：
执行 make
以及 sudo make install
再测试：
1）首先创建 该摄像头的 输出 以及 端口号（这里 是 8080）：
/usr/local/bin/mjpg_streamer -i "/usr/local/lib/mjpg-streamer/input_uvc.so -n -f 30 -r 1280x720" -o "/usr/local/lib/mjpg-streamer/output_http.so -p 8080 -w /usr/local/share/mjpg-streamer/ww
2）然后使用javastrip地址（http://<树莓派地址（网络地址或者物理地址）>:<端口号>）该地址 用网络 地址需要在覆盖范围区域内，用物理地址时，树莓派需要接网线（这里会影响小车，建议使用网络地址）
或者编写 .html文件(后面有该编码)
9.树莓派 自启动程序
Nuhup查看是否有
如有没有安装
然后使用命令
nohup python test.py > pomelo.js.log &
将test.py挂到后台运行
然后将该命令放在 /etc目录下
rc.local文件中 即sudo vim rc.local(因为是系统文件 需要权限)
10.scrot屏幕截图
1.安装
sudo apt-get install scrot
2.查看scrot -v
3使用scrot -s（选择区域截图）
11.wobot以及edgeboardAI和STM32的连接
在固件按最上面到下面依次WOBOT和edgeboardAI，STM的顺序依次连接好的基础上。
1.双头USB线连接edgeboardAI的USB接口
2.绿色的电源线一端连接STM一段连接edgeboardAI的电源线
3.然后个驱动马达和传感器通过网络数据线连接WOBOT的M1~M4和P1~P4
4.连接好后STM32和WOBOT都需要接通圆孔电源线，其中STM32给edgeboardAI供电，wobot通过自己的电源线供电。
5.如果需要远程，则将STM32电源线换成2500MA的电池，将WOBOT换成800MA+的电池。
2.界面设计
实际的界面截图，标出各控件的名称
Jacvstrip地址
Html 语言 firefox web.html ?获知谷歌 chromium-browser web.html
Web.html 文件
<!DOCTYPE html>
<html>
<head>
<title>实时视频</title>
<style>
?????#webcam{
??????????width: 80%;
??????????height: 80%;
??????????display: block;
??????????margin: 10% auto;
??????????text-align: center;
??????????position: relative;
?????}
?????#webcam img{
?????????width: 100%;
?????????height: auto;
?????????display: block;
?????????margin: 0 auto;
?????}
</style>
</head>
<body>
<div id="webcam">
?????<div>
?????</div>
</div>
<script type="text/javascript">
????var imageNr = 0; // 图片的索引号
????var finished = new Array(); // 下载图片的队列
????var paused = false; //
?????
????function createImageLayer() {
??????var img = new Image();
??????img.style.position = "absolute";
??????img.style.zIndex = -1;
??????img.onload = imageOnload;
??????img.onclick = imageOnclick;
?????????//填你对应的ip和端口
??????img.src = "http://172.19.8.176:8080/?action=snapshot&n=" + (++imageNr);
??????var webcam = document.getElementById("webcam");
??????webcam.insertBefore(img, webcam.firstChild);
????}
?????
?????
????function imageOnload() {
??????this.style.zIndex = imageNr;
??????while (1 < finished.length) {
????????var del = finished.shift(); // 删除旧照片
????????del.parentNode.removeChild(del);
??????}
??????finished.push(this);
??????if (!paused) createImageLayer();
????}
?????
????function imageOnclick() {
??????paused = !paused;
??????if (!paused) createImageLayer();
????}
????createImageLayer()
</script>
</body>
</html>
3.功能和代码
3.1.智能小车的电机驱动
3.1.1.功能简介
驱动 小车 前进 后退 左转 右转 左上走 右上走等：
前进和后退，左右两边的方向都朝前或朝后，速度一致；
原地顺时针旋转时，左边轮子前进，右边轮子后退，速度一致；
原地逆时针旋转时，左边轮子后退，右边轮子前进，速度一致；
偏左前进时，左右两边的方向都朝前，左轮速度比右轮速度慢一点；
偏右前进时，左右两边的方向都朝前，左轮速度比右轮速度快一点；
偏左后退时，左右两边的方向都朝后，左轮速度比右轮速度慢一点；
偏右后退时，左右两边的方向都朝后，左轮速度比右轮速度快一点；
3.1.2.操作截图
3.1.3.代码说明
#初始化代码
#-*- coding:UTF-8 -*-
import RPi.GPIO as GPIO
import time
#小车电机引脚定义
IN1 = 20
IN2 = 21
IN3 = 19
IN4 = 26
ENA = 16
ENB = 13
#设置GPIO口为BCM编码方式
GPIO.setmode(GPIO.BCM)
#忽略警告信息
GPIO.setwarnings(False)
#电机引脚初始化操作
def motor_init():
????global pwm_ENA
????global pwm_ENB
????GPIO.setup(ENA,GPIO.OUT,initial=GPIO.HIGH)
????GPIO.setup(IN1,GPIO.OUT,initial=GPIO.LOW)
????GPIO.setup(IN2,GPIO.OUT,initial=GPIO.LOW)
????GPIO.setup(ENB,GPIO.OUT,initial=GPIO.HIGH)
????GPIO.setup(IN3,GPIO.OUT,initial=GPIO.LOW)
????GPIO.setup(IN4,GPIO.OUT,initial=GPIO.LOW)
????#设置pwm引脚和频率为2000hz
????pwm_ENA = GPIO.PWM(ENA, 2000)
pwm_ENB = GPIO.PWM(ENB, 2000)
#控制驱动代码
#小车前进
def run():
????GPIO.output(IN1, GPIO.HIGH)
????GPIO.output(IN2, GPIO.LOW)
????GPIO.output(IN3, GPIO.HIGH)
????GPIO.output(IN4, GPIO.LOW)
????pwm_ENA.ChangeDutyCycle(CarSpeedControl)
????pwm_ENB.ChangeDutyCycle(CarSpeedControl)
#小车后退
def back():
????GPIO.output(IN1, GPIO.LOW)
????GPIO.output(IN2, GPIO.HIGH)
????GPIO.output(IN3, GPIO.LOW)
????GPIO.output(IN4, GPIO.HIGH)
????pwm_ENA.ChangeDutyCycle(CarSpeedControl)
????pwm_ENB.ChangeDutyCycle(CarSpeedControl)
#小车左转
def left():
????GPIO.output(IN1, GPIO.LOW)
????GPIO.output(IN2, GPIO.LOW)
????GPIO.output(IN3, GPIO.HIGH)
????GPIO.output(IN4, GPIO.LOW)
????pwm_ENA.ChangeDutyCycle(CarSpeedControl)
????pwm_ENB.ChangeDutyCycle(CarSpeedControl)
#小车右转
def right():
????GPIO.output(IN1, GPIO.HIGH)
????GPIO.output(IN2, GPIO.LOW)
????GPIO.output(IN3, GPIO.LOW)
????GPIO.output(IN4, GPIO.LOW)
????pwm_ENA.ChangeDutyCycle(CarSpeedControl)
????pwm_ENB.ChangeDutyCycle(CarSpeedControl)
#小车原地左转
def spin_left():
????GPIO.output(IN1, GPIO.LOW)
????GPIO.output(IN2, GPIO.HIGH)
????GPIO.output(IN3, GPIO.HIGH)
????GPIO.output(IN4, GPIO.LOW)
????pwm_ENA.ChangeDutyCycle(CarSpeedControl)
????pwm_ENB.ChangeDutyCycle(CarSpeedControl)
#小车原地右转
def spin_right():
????GPIO.output(IN1, GPIO.HIGH)
????GPIO.output(IN2, GPIO.LOW)
????GPIO.output(IN3, GPIO.LOW)
????GPIO.output(IN4, GPIO.HIGH)
????pwm_ENA.ChangeDutyCycle(CarSpeedControl)
????pwm_ENB.ChangeDutyCycle(CarSpeedControl)
#小车沿左前轮前进
def upleft():
????GPIO.output(IN1, GPIO.HIGH)
????GPIO.output(IN2, GPIO.LOW)
????GPIO.output(IN3, GPIO.HIGH)
????GPIO.output(IN4, GPIO.LOW)
????pwm_ENA.ChangeDutyCycle(CarSpeedControl-20)
????pwm_ENB.ChangeDutyCycle(CarSpeedControl+20) ???
#小车沿左后方后退
def downleft():
????GPIO.output(IN1, GPIO.LOW)
????GPIO.output(IN2, GPIO.HIGH)
????GPIO.output(IN3, GPIO.LOW)
????GPIO.output(IN4, GPIO.HIGH)
????pwm_ENA.ChangeDutyCycle(CarSpeedControl-20)
????pwm_ENB.ChangeDutyCycle(CarSpeedControl+20)
#小车沿右上方前进
def upright():
????GPIO.output(IN1, GPIO.HIGH)
????GPIO.output(IN2, GPIO.LOW)
????GPIO.output(IN3, GPIO.HIGH)
????GPIO.output(IN4, GPIO.LOW)
????pwm_ENA.ChangeDutyCycle(CarSpeedControl+20)
????pwm_ENB.ChangeDutyCycle(CarSpeedControl-20) ??
#小车沿右下方后退
def downright():
????GPIO.output(IN1, GPIO.LOW)
????GPIO.output(IN2, GPIO.HIGH)
????GPIO.output(IN3, GPIO.LOW)
????GPIO.output(IN4, GPIO.HIGH)
????pwm_ENA.ChangeDutyCycle(CarSpeedControl+20)
????pwm_ENB.ChangeDutyCycle(CarSpeedControl-20)
#小车停止
def brake():
???GPIO.output(IN1, GPIO.LOW)
???GPIO.output(IN2, GPIO.LOW)
???GPIO.output(IN3, GPIO.LOW)
???GPIO.output(IN4, GPIO.LOW)0
time.sleep(2)
#try/except语句用来检测try语句块中的错误
#从而让except语句捕捉异常信息并处理
try:
????motor_init()
????while True:
????????run()
except KeyboardInterrupt:
????pass
pwm_ENA.stop()
pwm_ENB.stop()
GPIO.cleanup()
3.2.摄像头的使用，以及舵机与摄像头平台的转动
3.2.1.功能简介
1.USB摄像头
初始化摄像头
USB摄像头工作时,读取一帧图像
显示图像窗口在树莓派的屏幕上
或用命令行在 web 网页上显示 摄像图像
2.舵机的控制
调整舵机PWM的占空比，将占空比的改变变成
具体角度的变化
或用命令行在 web 网页上显示 摄像图像
3.2.2.操作截图
代码方式
运行代码截图
Web方式 ??MJPG-Streamer
实时视频接收
启用树梅派摄像
*.html web启用
javascript地址模式
3.2.3.代码说明
列出主要的源码，对重要语句用文字注释
# 1舵机的控制
#-*- coding:UTF-8 -*-
#舵机转动控制舵机采用系统的pwm库 对占空比进行更改
import RPi.GPIO as GPIO
import time
#RGB三色灯引脚定义
LED_R = 22
LED_G = 27
LED_B = 24
#舵机引脚定义
ServoPin = 11
#设置GPIO口为BCM编码方式
GPIO.setmode(GPIO.BCM)
#忽略警告信息
GPIO.setwarnings(False)
#RGB三色灯初始化为输出模式
#舵机引脚设置为输出模式
def init():
????global pwm_servo
????GPIO.setup(LED_R, GPIO.OUT)
????GPIO.setup(LED_G, GPIO.OUT)
????GPIO.setup(LED_B, GPIO.OUT)
????GPIO.setup(ServoPin, GPIO.OUT)
????#设置pwm引脚和频率为50hz
????pwm_servo = GPIO.PWM(ServoPin, 50)
????pwm_servo.start(0)
#根据转动的角度来点亮相应的颜色
def corlor_light(pos):
????if pos > 150:
????????GPIO.output(LED_R, GPIO.HIGH)
GPIO.output(LED_G, GPIO.LOW)
GPIO.output(LED_B, GPIO.LOW)
????elif pos > 125:
GPIO.output(LED_R, GPIO.LOW)
GPIO.output(LED_G, GPIO.HIGH)
GPIO.output(LED_B, GPIO.LOW)
????elif pos >100:
????????GPIO.output(LED_R, GPIO.LOW)
GPIO.output(LED_G, GPIO.LOW)
GPIO.output(LED_B, GPIO.HIGH)
????elif pos > 75:
GPIO.output(LED_R, GPIO.HIGH)
GPIO.output(LED_G, GPIO.HIGH)
GPIO.output(LED_B, GPIO.LOW)
????elif pos > 50:
GPIO.output(LED_R, GPIO.LOW)
GPIO.output(LED_G, GPIO.HIGH)
GPIO.output(LED_B, GPIO.HIGH)
????elif pos > 25:
????????GPIO.output(LED_R, GPIO.HIGH)
GPIO.output(LED_G, GPIO.LOW)
GPIO.output(LED_B, GPIO.HIGH)
????elif pos > 0:
????????GPIO.output(LED_R, GPIO.HIGH)
GPIO.output(LED_G, GPIO.HIGH)
GPIO.output(LED_B, GPIO.HIGH)
????else :
????????GPIO.output(LED_R, GPIO.LOW)
GPIO.output(LED_G, GPIO.LOW)
GPIO.output(LED_B, GPIO.LOW)
#舵机来回转动
def servo_control_color():
????for pos in range(181):
????????pwm_servo.ChangeDutyCycle(2.5 + 10 * pos/180)
corlor_light(pos)
time.sleep(0.009)
????for pos in reversed(range(181)):
????????pwm_servo.ChangeDutyCycle(2.5 + 10 * pos/180)
corlor_light(pos)
time.sleep(0.009)
#延时2s
time.sleep(2)
#try/except语句用来检测try语句块中的错误，
#从而让except语句捕获异常信息并处理。
try:
????init()
????#舵机初始化向前
????pwm_servo.ChangeDutyCycle(2.5 + 10 * 90/180)
????while True:
? servo_control_color()
except KeyboardInterrupt:
????pass
pwm_servo.stop()
GPIO.cleanup()
#2 打开usb摄像头读取帧照片
import CV2
import matplotlib.pyplot as plt
#opencv调用csi摄像头优先0，然后usb按顺序排列下去
capture = CV2.VideoCapture(0)
# 获取一帧
ret, frame = capture.read()
plt.imshow(frame[:,:,::-1])#BGRtoRGB
plt.show()
# 释放资源
capture.release()
除了程序打开
还可以使用 命令行，打开USB摄像头进行录制和拍摄
cd mjpg-streamer-master/mjpg-streamer-experimental(切换到树莓派上 摄像头软件目录)
./mjpg_stream -i ”./input_uvc.so” -o ”./output_http.so -w ./www”（命令启动 usb摄像头 这里实例 主要用了USB摄像头 没有用 树莓派摄像头）
或者 ./start.sh(bash 命令快捷启动)
注意开启后 保持终端 防止 摄像头中断
#实现视频接受 html
<!DOCTYPE html>
<html>
<head>
<title>实时视频</title>
<style>
?????#webcam{
??????????width: 80%;
??????????height: 80%;
??????????display: block;
??????????margin: 10% auto;
??????????text-align: center;
??????????position: relative;
?????}
?????#webcam img{
?????????width: 100%;
?????????height: auto;
?????????display: block;
?????????margin: 0 auto;
?????}
</style>
</head>
<body>
<div id="webcam">
?????<div>
?????</div>
</div>
<script type="text/javascript">
????var imageNr = 0; // 图片的索引号
????var finished = new Array(); // 下载图片的队列
????var paused = false; //
?????
????function createImageLayer() {
??????var img = new Image();
??????img.style.position = "absolute";
??????img.style.zIndex = -1;
??????img.onload = imageOnload;
??????img.onclick = imageOnclick;
?????????//填你对应的ip和端口
??????img.src = "http://172.19.8.176:8080/?action=snapshot&n=" + (++imageNr);
??????var webcam = document.getElementById("webcam");
??????webcam.insertBefore(img, webcam.firstChild);
????}
?????
?????
????function imageOnload() {
??????this.style.zIndex = imageNr;
??????while (1 < finished.length) {
????????var del = finished.shift(); // 删除旧照片
????????del.parentNode.removeChild(del);
??????}
??????finished.push(this);
??????if (!paused) createImageLayer();
????}
?????
????function imageOnclick() {
??????paused = !paused;
??????if (!paused) createImageLayer();
????}
????createImageLayer()
</script>
</body>
</html>
3.3.蓝牙通信控制驱动
3.3.1.功能简介
简要说明该功能
1.蓝牙通信控制驱动
获取手柄传入的蓝牙信息，
调整案件和左边转轴的范围
，根据获取到的案件信息调取对应的驱动功能
按键
（-1，0）向左
（1，0）右
（0，-1）上
（0，1）下
左右转轴
Axi1 axi2 axi3 axi4 axi5
测试
3.3.2.操作截图
该功能的操作截图
3.3.3.代码说明
列出主要的源码，对重要语句用文字注释
自己添加功能章节
#-*- coding:UTF-8 -*-
import RPi.GPIO as GPIO
import time
import spidev
import pygame
????
#手柄按键定义
PSB_forward = 1
PSB_back = ?2
PSB_Right = ?3
PSB_left = 4
PSB_stop = 5
#PS2引脚设置
#回发过来的后4个数据是摇杆的数据
#小车运行状态值定义
enSTOP = 0
enRUN =1
enBACK = 2
enLEFT = 3
enRIGHT = 4
enTLEFT =5
enTRIGHT = 6
enUPLEFT = 7
enUPRIGHT = 8
enDOWNLEFT = 9
enDOWNRIGHT = 10
#小车电机引脚定义
IN1 = 20
IN2 = 21
IN3 = 19
IN4 = 26
ENA = 16
ENB = 13
#舵机引脚定义
ServoPin = 11
#蜂鸣器引脚定义
buzzer = 8
#灭火电机引脚设置
OutfirePin = 2
#RGB三色灯引脚定义
LED_R = 22
LED_G = 27
LED_B = 24
#小车速度变量
CarSpeedControl = 50
#小车舵机状态变量
g_ServoState = 0
#设置GPIO口为BCM编码方式
GPIO.setmode(GPIO.BCM)
#忽略警告信息
GPIO.setwarnings(False)
#电机引脚初始化为输出模式
#RGB三色灯,舵机引脚初始化
def init():
????global pwm_ENA
????global pwm_ENB
????global pwm_servo
????global pwm_rled
????global pwm_gled
????global pwm_bled ??
????GPIO.setup(ENA,GPIO.OUT,initial=GPIO.HIGH)
????GPIO.setup(IN1,GPIO.OUT,initial=GPIO.LOW)
????GPIO.setup(IN2,GPIO.OUT,initial=GPIO.LOW)
????GPIO.setup(ENB,GPIO.OUT,initial=GPIO.HIGH)
????GPIO.setup(IN3,GPIO.OUT,initial=GPIO.LOW)
????GPIO.setup(IN4,GPIO.OUT,initial=GPIO.LOW)
????GPIO.setup(buzzer,GPIO.OUT,initial=GPIO.HIGH)
????GPIO.setup(OutfirePin,GPIO.OUT,initial=GPIO.HIGH)
????GPIO.setup(LED_R, GPIO.OUT)
????GPIO.setup(LED_G, GPIO.OUT)
????GPIO.setup(LED_B, GPIO.OUT)
????GPIO.setup(ServoPin, GPIO.OUT)
????#设置pwm引脚和频率为2000hz
????pwm_ENA = GPIO.PWM(ENA, 2000)
????pwm_ENB = GPIO.PWM(ENB, 2000)
????pwm_ENA.start(0)
????pwm_ENB.start(0)
????#设置舵机的频率和起始占空比
????pwm_servo = GPIO.PWM(ServoPin, 50)
????pwm_servo.start(0)
????pwm_rled = GPIO.PWM(LED_R, 1000)
????pwm_gled = GPIO.PWM(LED_G, 1000)
????pwm_bled = GPIO.PWM(LED_B, 1000)
????pwm_rled.start(0)
????pwm_gled.start(0)
????pwm_bled.start(0)
#小车前进
def run():
????GPIO.output(IN1, GPIO.HIGH)
????GPIO.output(IN2, GPIO.LOW)
????GPIO.output(IN3, GPIO.HIGH)
????GPIO.output(IN4, GPIO.LOW)
????pwm_ENA.ChangeDutyCycle(CarSpeedControl)
????pwm_ENB.ChangeDutyCycle(CarSpeedControl)
#小车后退
def back():
????GPIO.output(IN1, GPIO.LOW)
????GPIO.output(IN2, GPIO.HIGH)
????GPIO.output(IN3, GPIO.LOW)
????GPIO.output(IN4, GPIO.HIGH)
????pwm_ENA.ChangeDutyCycle(CarSpeedControl)
????pwm_ENB.ChangeDutyCycle(CarSpeedControl)
#小车左转
def left():
????GPIO.output(IN1, GPIO.LOW)
????GPIO.output(IN2, GPIO.LOW)
????GPIO.output(IN3, GPIO.HIGH)
????GPIO.output(IN4, GPIO.LOW)
????pwm_ENA.ChangeDutyCycle(CarSpeedControl)
????pwm_ENB.ChangeDutyCycle(CarSpeedControl)
#小车右转
def right():
????GPIO.output(IN1, GPIO.HIGH)
????GPIO.output(IN2, GPIO.LOW)
????GPIO.output(IN3, GPIO.LOW)
????GPIO.output(IN4, GPIO.LOW)
????pwm_ENA.ChangeDutyCycle(CarSpeedControl)
????pwm_ENB.ChangeDutyCycle(CarSpeedControl)
#小车原地左转
def spin_left():
????GPIO.output(IN1, GPIO.LOW)
????GPIO.output(IN2, GPIO.HIGH)
????GPIO.output(IN3, GPIO.HIGH)
????GPIO.output(IN4, GPIO.LOW)
????pwm_ENA.ChangeDutyCycle(CarSpeedControl)
????pwm_ENB.ChangeDutyCycle(CarSpeedControl)
#小车原地右转
def spin_right():
????GPIO.output(IN1, GPIO.HIGH)
????GPIO.output(IN2, GPIO.LOW)
????GPIO.output(IN3, GPIO.LOW)
????GPIO.output(IN4, GPIO.HIGH)
????pwm_ENA.ChangeDutyCycle(CarSpeedControl)
????pwm_ENB.ChangeDutyCycle(CarSpeedControl)
#小车沿左前轮前进
def upleft():
????GPIO.output(IN1, GPIO.HIGH)
????GPIO.output(IN2, GPIO.LOW)
????GPIO.output(IN3, GPIO.HIGH)
????GPIO.output(IN4, GPIO.LOW)
????pwm_ENA.ChangeDutyCycle(CarSpeedControl-20)
????pwm_ENB.ChangeDutyCycle(CarSpeedControl+20) ???
#小车沿左后方后退
def downleft():
????GPIO.output(IN1, GPIO.LOW)
????GPIO.output(IN2, GPIO.HIGH)
????GPIO.output(IN3, GPIO.LOW)
????GPIO.output(IN4, GPIO.HIGH)
????pwm_ENA.ChangeDutyCycle(CarSpeedControl-20)
????pwm_ENB.ChangeDutyCycle(CarSpeedControl+20)
#小车沿右上方前进
def upright():
????GPIO.output(IN1, GPIO.HIGH)
????GPIO.output(IN2, GPIO.LOW)
????GPIO.output(IN3, GPIO.HIGH)
????GPIO.output(IN4, GPIO.LOW)
????pwm_ENA.ChangeDutyCycle(CarSpeedControl+20)
????pwm_ENB.ChangeDutyCycle(CarSpeedControl-20) ??
#小车沿右下方后退
def downright():
????GPIO.output(IN1, GPIO.LOW)
????GPIO.output(IN2, GPIO.HIGH)
????GPIO.output(IN3, GPIO.LOW)
????GPIO.output(IN4, GPIO.HIGH)
????pwm_ENA.ChangeDutyCycle(CarSpeedControl+20)
????pwm_ENB.ChangeDutyCycle(CarSpeedControl-20)
#小车停止
def brake():
???GPIO.output(IN1, GPIO.LOW)
???GPIO.output(IN2, GPIO.LOW)
???GPIO.output(IN3, GPIO.LOW)
???GPIO.output(IN4, GPIO.LOW)
???
#小车鸣笛
def whistle():
????GPIO.output(buzzer, GPIO.LOW)
????time.sleep(0.1)
????GPIO.output(buzzer, GPIO.HIGH)
????time.sleep(0.001)
#舵机旋转到指定角度
def servo_appointed_detection(pos):
????for i in range(18):
????????pwm_servo.ChangeDutyCycle(2.5 + 10 * pos/180)
????????time.sleep(0.02) #等待20ms周期结束
????????pwm_servo.ChangeDutyCycle(0) #归零信号
#七彩灯亮指定颜色
def color_led_pwm(iRed,iGreen, iBlue):
????v_red = (100*iRed)/255
????v_green = (100*iGreen)/255
????v_blue = (100*iBlue)/255
????pwm_rled.ChangeDutyCycle(v_red)
????pwm_gled.ChangeDutyCycle(v_green)
????pwm_bled.ChangeDutyCycle(v_blue)
def pygame_get():
????for event_ in pygame.event.get():
????????# 退出事件
????????if event_.type == pygame.QUIT:
????????????done = True
???
# 轴转动事件
????????elif event_.type == pygame.JOYAXISMOTION:
????????????axes = joystick.get_numaxes()
????????????# 获取axis-0 | axis-1轴状态信息
????????????axis0 = joystick.get_axis(0)
????????????axis1 = joystick.get_axis(1)
????for i in range(2):
x="axis"+str(i)
???? print("axis " + str(i) +": " + str(eval(x)))
????if str(axis0) == "-1.0":
left()
????elif str(axis0) >= "0.99996948" and str(axis0)[0] != '-':
right()
????elif str(axis1) == "-1.0":
run()
????elif str(axis1) >= "0.9999" and str(axis0)[0] != '-':
back()
????else:
? brake()
????????# 方向键改变事件
????????elif event_.type == pygame.JOYHATMOTION:
????????????hats = joystick.get_numhats()
????????????# 获取所有方向键状态信息
????????????for i in range(hats):
????????????????hat = joystick.get_hat(i)
????????????????print(str(hat))
????????????????if str(hat) == "(0, 1)":
????run()
elif str(hat) == "(0, -1)":
????back()
elif str(hat) == "(1, 0)":
????right()
elif str(hat) == "(-1, 0)":
????left()
else:
????brake()
????joystick_count = pygame.joystick.get_count()
????return 1
while True:
????#蜂鸣器响连接开始
????GPIO.setup(buzzer,GPIO.OUT,initial=GPIO.LOW)
????time.sleep(1.5)
????GPIO.setup(buzzer,GPIO.OUT,initial=GPIO.HIGH)
????print("two minutes delay start")
????time.sleep(15.5)
????print("End of hte time delay")
????# 模块初始化
????pygame.init()
????pygame.joystick.init()
????
????
?????
????
????# 手柄对象初始化 # 若只连接了一个手柄，此处带入的参数一般都是0
????try:
joystick = pygame.joystick.Joystick(0)
????except pygame.error:
???? continue
???
????joystick.init()
????
????done = False
????clock = pygame.time.Clock()
????if pygame_get() == 1:
break
try:
????init()
????
????
????while True:
????????global CarSpeedControl ?#to global impact control
global g_Carstate ##to global impact control
????????a=pygame_get()
????????#必要的延时避免过于频繁发送手柄指令 ?
time.sleep(0.5)
except KeyboardInterrupt:
????pass
pwm_ENA.stop()
pwm_ENB.stop()
pwm_rled.stop()
pwm_gled.stop()
pwm_bled.stop()
pwm_servo.stop()
GPIO.cleanup()
????
??
3.4.蓝牙通信控制舵机
3.4.1.功能简介
1.蓝牙通信控制舵机
调整舵机PWM的占空比，将占空比的改变变成
具体角度的变化。
然后同理获取手柄传入的蓝牙信息，
调整案件和左边转轴的范围
，根据获取到的案件信息调取对应的
可以精确到2度的舵机转动函数
1当舵机转到右边不能再转动 时 会出现 -1 此时转向最左边
2当舵机转到左边不能再转动 时 会出现 最大值 此时转向最右边
这里改用 按键来实现，避免转轴的 重复延迟效应，即会触发2~3次命令，得消抖，用按键不存在此类问题
3.4.2.操作截图
该功能的操作截图
3.4.3.代码说明
#-*- coding:UTF-8 -*-
import RPi.GPIO as GPIO
import time
import spidev
import pygame
????
#小车运行状态值定义
enSTOP = 0
enRUN =1
enBACK = 2
enLEFT = 3
enRIGHT = 4
enTLEFT =5
enTRIGHT = 6
enUPLEFT = 7
enUPRIGHT = 8
enDOWNLEFT = 9
enDOWNRIGHT = 10
#小车电机引脚定义
IN1 = 20
IN2 = 21
IN3 = 19
IN4 = 26
ENA = 16
ENB = 13
#舵机引脚定义
ServoPin = 11
#蜂鸣器引脚定义
buzzer = 8
#灭火电机引脚设置
OutfirePin = 2
#RGB三色灯引脚定义
LED_R = 22
LED_G = 27
LED_B = 24
#小车速度变量
CarSpeedControl = 50
#小车舵机状态变量
g_ServoState = 0
#设置GPIO口为BCM编码方式
GPIO.setmode(GPIO.BCM)
#忽略警告信息
GPIO.setwarnings(False)
#电机引脚初始化为输出模式
#RGB三色灯,舵机引脚初始化
def init():
????global pwm_ENA
????global pwm_ENB
????global pwm_servo
????global pwm_rled
????global pwm_gled
????global pwm_bled ??
????GPIO.setup(ENA,GPIO.OUT,initial=GPIO.HIGH)
????GPIO.setup(IN1,GPIO.OUT,initial=GPIO.LOW)
????GPIO.setup(IN2,GPIO.OUT,initial=GPIO.LOW)
????GPIO.setup(ENB,GPIO.OUT,initial=GPIO.HIGH)
????GPIO.setup(IN3,GPIO.OUT,initial=GPIO.LOW)
????GPIO.setup(IN4,GPIO.OUT,initial=GPIO.LOW)
????GPIO.setup(buzzer,GPIO.OUT,initial=GPIO.HIGH)
????
????GPIO.setup(LED_R, GPIO.OUT)
????GPIO.setup(LED_G, GPIO.OUT)
????GPIO.setup(LED_B, GPIO.OUT)
????GPIO.setup(ServoPin, GPIO.OUT)
????#设置pwm引脚和频率为2000hz
????pwm_ENA = GPIO.PWM(ENA, 2000)
????pwm_ENB = GPIO.PWM(ENB, 2000)
????pwm_ENA.start(0)
????pwm_ENB.start(0)
????#设置舵机的频率和起始占空比
????pwm_servo = GPIO.PWM(ServoPin, 50)
????pwm_servo.start(0)
????pwm_rled = GPIO.PWM(LED_R, 1000)
????pwm_gled = GPIO.PWM(LED_G, 1000)
????pwm_bled = GPIO.PWM(LED_B, 1000)
????pwm_rled.start(0)
????pwm_gled.start(0)
????pwm_bled.start(0)
#小车前进
def run():
????GPIO.output(IN1, GPIO.HIGH)
????GPIO.output(IN2, GPIO.LOW)
????GPIO.output(IN3, GPIO.HIGH)
????GPIO.output(IN4, GPIO.LOW)
????pwm_ENA.ChangeDutyCycle(CarSpeedControl)
????pwm_ENB.ChangeDutyCycle(CarSpeedControl)
#小车后退
def back():
????GPIO.output(IN1, GPIO.LOW)
????GPIO.output(IN2, GPIO.HIGH)
????GPIO.output(IN3, GPIO.LOW)
????GPIO.output(IN4, GPIO.HIGH)
????pwm_ENA.ChangeDutyCycle(CarSpeedControl)
????pwm_ENB.ChangeDutyCycle(CarSpeedControl)
#小车左转
def left():
????GPIO.output(IN1, GPIO.LOW)
????GPIO.output(IN2, GPIO.LOW)
????GPIO.output(IN3, GPIO.HIGH)
????GPIO.output(IN4, GPIO.LOW)
????pwm_ENA.ChangeDutyCycle(CarSpeedControl)
????pwm_ENB.ChangeDutyCycle(CarSpeedControl)
#小车右转
def right():
????GPIO.output(IN1, GPIO.HIGH)
????GPIO.output(IN2, GPIO.LOW)
????GPIO.output(IN3, GPIO.LOW)
????GPIO.output(IN4, GPIO.LOW)
????pwm_ENA.ChangeDutyCycle(CarSpeedControl)
????pwm_ENB.ChangeDutyCycle(CarSpeedControl)
#小车原地左转
def spin_left():
????GPIO.output(IN1, GPIO.LOW)
????GPIO.output(IN2, GPIO.HIGH)
????GPIO.output(IN3, GPIO.HIGH)
????GPIO.output(IN4, GPIO.LOW)
????pwm_ENA.ChangeDutyCycle(CarSpeedControl)
????pwm_ENB.ChangeDutyCycle(CarSpeedControl)
#小车原地右转
def spin_right():
????GPIO.output(IN1, GPIO.HIGH)
????GPIO.output(IN2, GPIO.LOW)
????GPIO.output(IN3, GPIO.LOW)
????GPIO.output(IN4, GPIO.HIGH)
????pwm_ENA.ChangeDutyCycle(CarSpeedControl)
????pwm_ENB.ChangeDutyCycle(CarSpeedControl)
#小车沿左前轮前进
def upleft():
????GPIO.output(IN1, GPIO.HIGH)
????GPIO.output(IN2, GPIO.LOW)
????GPIO.output(IN3, GPIO.HIGH)
????GPIO.output(IN4, GPIO.LOW)
????pwm_ENA.ChangeDutyCycle(CarSpeedControl-20)
????pwm_ENB.ChangeDutyCycle(CarSpeedControl+20) ???
#小车沿左后方后退
def downleft():
????GPIO.output(IN1, GPIO.LOW)
????GPIO.output(IN2, GPIO.HIGH)
????GPIO.output(IN3, GPIO.LOW)
????GPIO.output(IN4, GPIO.HIGH)
????pwm_ENA.ChangeDutyCycle(CarSpeedControl-20)
????pwm_ENB.ChangeDutyCycle(CarSpeedControl+20)
#小车沿右上方前进
def upright():
????GPIO.output(IN1, GPIO.HIGH)
????GPIO.output(IN2, GPIO.LOW)
????GPIO.output(IN3, GPIO.HIGH)
????GPIO.output(IN4, GPIO.LOW)
????pwm_ENA.ChangeDutyCycle(CarSpeedControl+20)
????pwm_ENB.ChangeDutyCycle(CarSpeedControl-20) ??
#小车沿右下方后退
def downright():
????GPIO.output(IN1, GPIO.LOW)
????GPIO.output(IN2, GPIO.HIGH)
????GPIO.output(IN3, GPIO.LOW)
????GPIO.output(IN4, GPIO.HIGH)
????pwm_ENA.ChangeDutyCycle(CarSpeedControl+20)
????pwm_ENB.ChangeDutyCycle(CarSpeedControl-20)
#小车停止
def brake():
???GPIO.output(IN1, GPIO.LOW)
???GPIO.output(IN2, GPIO.LOW)
???GPIO.output(IN3, GPIO.LOW)
???GPIO.output(IN4, GPIO.LOW)
???
#小车鸣笛
def whistle():
????GPIO.output(buzzer, GPIO.LOW)
????time.sleep(0.1)
????GPIO.output(buzzer, GPIO.HIGH)
????time.sleep(0.001)
#舵机旋转到指定角度
def servo_appointed_detection(pos):
????for i in range(18):
????????pwm_servo.ChangeDutyCycle(2.5 + 10 * pos/180)
????????time.sleep(0.02) #等待20ms周期结束
????????pwm_servo.ChangeDutyCycle(0) #归零信号
#七彩灯亮指定颜色
def color_led_pwm(iRed,iGreen, iBlue):
????v_red = (100*iRed)/255
????v_green = (100*iGreen)/255
????v_blue = (100*iBlue)/255
????pwm_rled.ChangeDutyCycle(v_red)
????pwm_gled.ChangeDutyCycle(v_green)
????pwm_bled.ChangeDutyCycle(v_blue)
m = 21
L=[3.75,6,7.5,15,17.5,22.5,25,30,35,35,37.5,45,50,52.5,60,65,67.5,75,80,82.5,90,97.5,98,105,112.5,113,120,127.5,128,135,142.5,143,150,157.5,158,165]
ff=0
def pygame_get():
????global m
????global ff
????for event_ in pygame.event.get():
????????# 退出事件
????????if event_.type == pygame.QUIT:
????????????done = True
???
# 轴转动事件
????????elif event_.type == pygame.JOYAXISMOTION:
????????????axes = joystick.get_numaxes()
????????????# 获取axis-0 | axis-1轴状态信息
????????????axis0 = joystick.get_axis(0)
????????????axis1 = joystick.get_axis(1)
????axis2 = joystick.get_axis(2)
????axis3 = joystick.get_axis(3)
????axis4 = joystick.get_axis(4)
????for i in range(5):
x="axis"+str(i)
???? print("axis " + str(i) +": " + str(eval(x)))
????if str(axis0) >= "-0.5" and str(axis0)[0] == '-':
left()
????elif str(axis0) >= "0.5" and str(axis0)[0] != '-':
right()
????elif str(axis1) >= "-0.5" and str(axis1)[0] == '-':
run()
????elif str(axis1) >= "0.5" and str(axis1)[0] != '-':
back()
????elif str(axis3) >= "-0.8" and str(axis3)[0] == '-' and ff == 0:
ff=1
m=m+1
servo_appointed_detection(L[m])
print(m)
time.sleep(0.5)
????elif str(axis3) >= "0.87" and str(axis3)[0] != '-' and ff == 0:
ff = 1
m=m-1
servo_appointed_detection(L[m])
print(m)
time.sleep(0.5)
????elif str(axis4) >= "-0.5" and str(axis4)[0] == '-':
GPIO.setup(OutfirePin,GPIO.OUT)
????elif str(axis4) >= "0.5" and str(axis4)[0] != '-':
GPIO.setup(OutfirePin,GPIO.OUT,initial=GPIO.LOW)
????else:
? brake()
GPIO.setup(OutfirePin,GPIO.OUT,initial=GPIO.HIGH)
ff = 0
????????# 方向键改变事件
????????elif event_.type == pygame.JOYHATMOTION:
????????????hats = joystick.get_numhats()
????????????# 获取所有方向键状态信息
????????????for i in range(hats):
????????????????hat = joystick.get_hat(i)
????????????????print(str(hat))
????????????????if str(hat) == "(0, 1)":
????run()
elif str(hat) == "(0, -1)":
????back()
elif str(hat) == "(1, 0)":
????right()
elif str(hat) == "(-1, 0)":
????left()
else:
????brake()
????joystick_count = pygame.joystick.get_count()
????return 1
while True:
????#蜂鸣器响连接开始
????GPIO.setup(buzzer,GPIO.OUT,initial=GPIO.HIGH)
????time.sleep(1.5)
????GPIO.setup(buzzer,GPIO.OUT,initial=GPIO.HIGH)
????print("15.5 s delay start")
????#time.sleep(15.5)
????print("End of hte time delay")
????# 模块初始化
????pygame.init()
????pygame.joystick.init()
????
????
?????
????
????# 手柄对象初始化 # 若只连接了一个手柄，此处带入的参数一般都是0
????try:
joystick = pygame.joystick.Joystick(0)
????except pygame.error:
???? continue
???
????joystick.init()
????
????done = False
????clock = pygame.time.Clock()
????if pygame_get() == 1:
break
try:
????init()
????
????servo_appointed_detection(90)
????while True:
????????global CarSpeedControl ?#to global impact control
global g_Carstate ##to global impact control
????????a=pygame_get()
????????#必要的延时避免过于频繁发送手柄指令 ?
time.sleep(0.05)
except KeyboardInterrupt:
????pass
pwm_ENA.stop()
pwm_ENB.stop()
pwm_rled.stop()
pwm_gled.stop()
pwm_bled.stop()
pwm_servo.stop()
GPIO.cleanup()
????
??
3.5.USB摄像头捕获道路图片同时获取方向信息
3.5.1.功能简介
1.USB摄像头捕获道路图片
sudo rasp-config 使能摄像头
捕获道路的图片，为了后面用该图片进行算法和模型的训练，让其能自动的在道路黄线内航线行，不偏航。
2.获取当前方向信息
在蓝牙控制驱动的基础上，接受通信所获得的信息，获取信息的同时捕获此时的摄像头采集到的图片，以此来辅助小车的方向。
3.5.2.操作截图
该功能的操作截图
3.5.3.代码说明
#摄像头 捕获
import cv2
import time
i=0
cap = cv2.VideoCapture(0)
while(1):
????ret ,frame = cap.read()
????time.sleep(2)
????cv2.imwrite('/home/pi/test_pictures/'+str(i)+'.jpg',frame)
????i+=1
cap.release()
cv2.destroyAllWindows()
#-*- coding:UTF-8 -*-
import RPi.GPIO as GPIO
import time
import spidev
import pygame
????
#手柄按键定义
PSB_forward = 1
PSB_back = ?2
PSB_Right = ?3
PSB_left = 4
PSB_stop = 5
#PS2引脚设置
#回发过来的后4个数据是摇杆的数据
#小车运行状态值定义
enSTOP = 0
enRUN =1
enBACK = 2
enLEFT = 3
enRIGHT = 4
enTLEFT =5
enTRIGHT = 6
enUPLEFT = 7
enUPRIGHT = 8
enDOWNLEFT = 9
enDOWNRIGHT = 10
#小车电机引脚定义
IN1 = 20
IN2 = 21
IN3 = 19
IN4 = 26
ENA = 16
ENB = 13
#舵机引脚定义
ServoPin = 11
#蜂鸣器引脚定义
buzzer = 8
#灭火电机引脚设置
OutfirePin = 2
#RGB三色灯引脚定义
LED_R = 22
LED_G = 27
LED_B = 24
#小车速度变量
CarSpeedControl = 50
#小车舵机状态变量
g_ServoState = 0
#设置GPIO口为BCM编码方式
GPIO.setmode(GPIO.BCM)
#忽略警告信息
GPIO.setwarnings(False)
#电机引脚初始化为输出模式
#RGB三色灯,舵机引脚初始化
def init():
????global pwm_ENA
????global pwm_ENB
????global pwm_servo
????global pwm_rled
????global pwm_gled
????global pwm_bled ??
????GPIO.setup(ENA,GPIO.OUT,initial=GPIO.HIGH)
????GPIO.setup(IN1,GPIO.OUT,initial=GPIO.LOW)
????GPIO.setup(IN2,GPIO.OUT,initial=GPIO.LOW)
????GPIO.setup(ENB,GPIO.OUT,initial=GPIO.HIGH)
????GPIO.setup(IN3,GPIO.OUT,initial=GPIO.LOW)
????GPIO.setup(IN4,GPIO.OUT,initial=GPIO.LOW)
????GPIO.setup(buzzer,GPIO.OUT,initial=GPIO.HIGH)
????GPIO.setup(OutfirePin,GPIO.OUT,initial=GPIO.HIGH)
????GPIO.setup(LED_R, GPIO.OUT)
????GPIO.setup(LED_G, GPIO.OUT)
????GPIO.setup(LED_B, GPIO.OUT)
????GPIO.setup(ServoPin, GPIO.OUT)
????#设置pwm引脚和频率为2000hz
????pwm_ENA = GPIO.PWM(ENA, 2000)
????pwm_ENB = GPIO.PWM(ENB, 2000)
????pwm_ENA.start(0)
????pwm_ENB.start(0)
????#设置舵机的频率和起始占空比
????pwm_servo = GPIO.PWM(ServoPin, 50)
????pwm_servo.start(0)
????pwm_rled = GPIO.PWM(LED_R, 1000)
????pwm_gled = GPIO.PWM(LED_G, 1000)
????pwm_bled = GPIO.PWM(LED_B, 1000)
????pwm_rled.start(0)
????pwm_gled.start(0)
????pwm_bled.start(0)
#小车前进
def run():
????GPIO.output(IN1, GPIO.HIGH)
????GPIO.output(IN2, GPIO.LOW)
????GPIO.output(IN3, GPIO.HIGH)
????GPIO.output(IN4, GPIO.LOW)
????pwm_ENA.ChangeDutyCycle(CarSpeedControl)
????pwm_ENB.ChangeDutyCycle(CarSpeedControl)
#小车后退
def back():
????GPIO.output(IN1, GPIO.LOW)
????GPIO.output(IN2, GPIO.HIGH)
????GPIO.output(IN3, GPIO.LOW)
????GPIO.output(IN4, GPIO.HIGH)
????pwm_ENA.ChangeDutyCycle(CarSpeedControl)
????pwm_ENB.ChangeDutyCycle(CarSpeedControl)
#小车左转
def left():
????GPIO.output(IN1, GPIO.LOW)
????GPIO.output(IN2, GPIO.LOW)
????GPIO.output(IN3, GPIO.HIGH)
????GPIO.output(IN4, GPIO.LOW)
????pwm_ENA.ChangeDutyCycle(CarSpeedControl)
????pwm_ENB.ChangeDutyCycle(CarSpeedControl)
#小车右转
def right():
????GPIO.output(IN1, GPIO.HIGH)
????GPIO.output(IN2, GPIO.LOW)
????GPIO.output(IN3, GPIO.LOW)
????GPIO.output(IN4, GPIO.LOW)
????pwm_ENA.ChangeDutyCycle(CarSpeedControl)
????pwm_ENB.ChangeDutyCycle(CarSpeedControl)
#小车原地左转
def spin_left():
????GPIO.output(IN1, GPIO.LOW)
????GPIO.output(IN2, GPIO.HIGH)
????GPIO.output(IN3, GPIO.HIGH)
????GPIO.output(IN4, GPIO.LOW)
????pwm_ENA.ChangeDutyCycle(CarSpeedControl)
????pwm_ENB.ChangeDutyCycle(CarSpeedControl)
#小车原地右转
def spin_right():
????GPIO.output(IN1, GPIO.HIGH)
????GPIO.output(IN2, GPIO.LOW)
????GPIO.output(IN3, GPIO.LOW)
????GPIO.output(IN4, GPIO.HIGH)
????pwm_ENA.ChangeDutyCycle(CarSpeedControl)
????pwm_ENB.ChangeDutyCycle(CarSpeedControl)
#小车沿左前轮前进
def upleft():
????GPIO.output(IN1, GPIO.HIGH)
????GPIO.output(IN2, GPIO.LOW)
????GPIO.output(IN3, GPIO.HIGH)
????GPIO.output(IN4, GPIO.LOW)
????pwm_ENA.ChangeDutyCycle(CarSpeedControl-20)
????pwm_ENB.ChangeDutyCycle(CarSpeedControl+20) ???
#小车沿左后方后退
def downleft():
????GPIO.output(IN1, GPIO.LOW)
????GPIO.output(IN2, GPIO.HIGH)
????GPIO.output(IN3, GPIO.LOW)
????GPIO.output(IN4, GPIO.HIGH)
????pwm_ENA.ChangeDutyCycle(CarSpeedControl-20)
????pwm_ENB.ChangeDutyCycle(CarSpeedControl+20)
#小车沿右上方前进
def upright():
????GPIO.output(IN1, GPIO.HIGH)
????GPIO.output(IN2, GPIO.LOW)
????GPIO.output(IN3, GPIO.HIGH)
????GPIO.output(IN4, GPIO.LOW)
????pwm_ENA.ChangeDutyCycle(CarSpeedControl+20)
????pwm_ENB.ChangeDutyCycle(CarSpeedControl-20) ??
#小车沿右下方后退
def downright():
????GPIO.output(IN1, GPIO.LOW)
????GPIO.output(IN2, GPIO.HIGH)
????GPIO.output(IN3, GPIO.LOW)
????GPIO.output(IN4, GPIO.HIGH)
????pwm_ENA.ChangeDutyCycle(CarSpeedControl+20)
????pwm_ENB.ChangeDutyCycle(CarSpeedControl-20)
#小车停止
def brake():
???GPIO.output(IN1, GPIO.LOW)
???GPIO.output(IN2, GPIO.LOW)
???GPIO.output(IN3, GPIO.LOW)
???GPIO.output(IN4, GPIO.LOW)
???
#小车鸣笛
def whistle():
????GPIO.output(buzzer, GPIO.LOW)
????time.sleep(0.1)
????GPIO.output(buzzer, GPIO.HIGH)
????time.sleep(0.001)
#舵机旋转到指定角度
def servo_appointed_detection(pos):
????for i in range(18):
????????pwm_servo.ChangeDutyCycle(2.5 + 10 * pos/180)
????????time.sleep(0.02) #等待20ms周期结束
????????pwm_servo.ChangeDutyCycle(0) #归零信号
#七彩灯亮指定颜色
def color_led_pwm(iRed,iGreen, iBlue):
????v_red = (100*iRed)/255
????v_green = (100*iGreen)/255
????v_blue = (100*iBlue)/255
????pwm_rled.ChangeDutyCycle(v_red)
????pwm_gled.ChangeDutyCycle(v_green)
????pwm_bled.ChangeDutyCycle(v_blue)
def pygame_get():
????for event_ in pygame.event.get():
????????# 退出事件
????????if event_.type == pygame.QUIT:
????????????done = True
???
# 轴转动事件
????????elif event_.type == pygame.JOYAXISMOTION:
????????????axes = joystick.get_numaxes()
????????????# 获取axis-0 | axis-1轴状态信息
????????????axis0 = joystick.get_axis(0)
????????????axis1 = joystick.get_axis(1)
????for i in range(2):
x="axis"+str(i)
???? print("axis " + str(i) +": " + str(eval(x)))
????if str(axis0) == "-1.0":
left()
????elif str(axis0) >= "0.99996948" and str(axis0)[0] != '-':
right()
????elif str(axis1) == "-1.0":
run()
????elif str(axis1) >= "0.9999" and str(axis0)[0] != '-':
back()
????else:
? brake()
????????# 方向键改变事件
????????elif event_.type == pygame.JOYHATMOTION:
????????????hats = joystick.get_numhats()
????????????# 获取所有方向键状态信息
????????????for i in range(hats):
????????????????hat = joystick.get_hat(i)
????????????????print(str(hat))
????????????????if str(hat) == "(0, 1)":
????run()
elif str(hat) == "(0, -1)":
????back()
elif str(hat) == "(1, 0)":
????right()
elif str(hat) == "(-1, 0)":
????left()
else:
????brake()
????joystick_count = pygame.joystick.get_count()
????return 1
while True:
????#蜂鸣器响连接开始
????GPIO.setup(buzzer,GPIO.OUT,initial=GPIO.LOW)
????time.sleep(1.5)
????GPIO.setup(buzzer,GPIO.OUT,initial=GPIO.HIGH)
????print("two minutes delay start")
????time.sleep(15.5)
????print("End of hte time delay")
????# 模块初始化
????pygame.init()
????pygame.joystick.init()
????
????
?????
????
????# 手柄对象初始化 # 若只连接了一个手柄，此处带入的参数一般都是0
????try:
joystick = pygame.joystick.Joystick(0)
????except pygame.error:
???? continue
???
????joystick.init()
????
????done = False
????clock = pygame.time.Clock()
????if pygame_get() == 1:
break
try:
????init()
????
????
????while True:
????????global CarSpeedControl ?#to global impact control
global g_Carstate ##to global impact control
????????a=pygame_get()
????????#必要的延时避免过于频繁发送手柄指令 ?
time.sleep(0.5)
except KeyboardInterrupt:
????pass
pwm_ENA.stop()
pwm_ENB.stop()
pwm_rled.stop()
pwm_gled.stop()
pwm_bled.stop()
pwm_servo.stop()
GPIO.cleanup()
????
??
3.6.根据超声波传感器距离调整
3.6.1.功能简介
1.根据超声波传感器距离调整
精确的完成打靶，可以使用超声波传感器进行侧面等方向的距离调整，保证后续的功能如刺靶等功能，能较精确的完成。
根据距离不同，进行距离的微调，保证每次能距离目标点固定距离。
具体操作：
首先 小车通过摄像头 训练 定在，地面目标点
然后 摄像头控制和靶心水平时
启用超声波传感器，感知距离靶心的距离
如果 距离靶心的距离 近了 采取 微调 先大方向 左上角 然后小方向右上角
如果 距离靶心的距离 远了 采取 微调 先大方向 右上角 然后小方向左上角
在前实现通信驱动，舵机驱动，摄像头捕获，实时监控，鸣笛
的基础上除了加入超声波传感器，
也加入了对加速，减速 驱动的设置（上线90，下线10（占空比）），保证距离平行移动的灵活，便于调试和测试。微调次数不因超过3次双方向轮回，10次单方向微调（考虑实际场地）
蓝牙微调控制程序启动：
这里改用 XYAB按键来实现，避免转轴的 重复延迟效应，即会触发2~3次命令，得消抖，用按键不存在此类问题
3.6.2.操作截图
该功能的操作截图
不同距离下获取的超声波传感器的参数（代码中 用公式 (((t2 - t1)* 340 / 2) * 100）转化为了单位为厘米的返回值
操作截图
3.6.3.代码说明
#-*- coding:UTF-8 -*-
import RPi.GPIO as GPIO
import time
import spidev
import pygame
#小车运行状态值定义
enSTOP = 0
enRUN =1
enBACK = 2
enLEFT = 3
enRIGHT = 4
enTLEFT =5
enTRIGHT = 6
enUPLEFT = 7
enUPRIGHT = 8
enDOWNLEFT = 9
enDOWNRIGHT = 10
#小车电机引脚定义
IN1 = 20
IN2 = 21
IN3 = 19
IN4 = 26
ENA = 16
ENB = 13
#舵机引脚定义
ServoPin = 11
#蜂鸣器引脚定义
buzzer = 8
#超声波引脚定义
EchoPin = 0
TrigPin = 1
#灭火电机引脚设置
OutfirePin = 2
#RGB三色灯引脚定义
LED_R = 22
LED_G = 27
LED_B = 24
#小车速度变量
CarSpeedControl = 50
#小车舵机状态变量
g_ServoState = 0
#设置GPIO口为BCM编码方式
GPIO.setmode(GPIO.BCM)
#忽略警告信息
GPIO.setwarnings(False)
#电机引脚初始化为输出模式
#RGB三色灯,舵机引脚初始化
def init():
????global pwm_ENA
????global pwm_ENB
????global pwm_servo
????global pwm_rled
????global pwm_gled
????global pwm_bled ??
????GPIO.setup(ENA,GPIO.OUT,initial=GPIO.HIGH)
????GPIO.setup(IN1,GPIO.OUT,initial=GPIO.LOW)
????GPIO.setup(IN2,GPIO.OUT,initial=GPIO.LOW)
????GPIO.setup(ENB,GPIO.OUT,initial=GPIO.HIGH)
????GPIO.setup(IN3,GPIO.OUT,initial=GPIO.LOW)
????GPIO.setup(IN4,GPIO.OUT,initial=GPIO.LOW)
????GPIO.setup(buzzer,GPIO.OUT,initial=GPIO.HIGH)
????
????GPIO.setup(LED_R, GPIO.OUT)
????GPIO.setup(LED_G, GPIO.OUT)
????GPIO.setup(LED_B, GPIO.OUT)
????GPIO.setup(ServoPin, GPIO.OUT)
????#设置pwm引脚和频率为2000hz
????pwm_ENA = GPIO.PWM(ENA, 2000)
????pwm_ENB = GPIO.PWM(ENB, 2000)
????pwm_ENA.start(0)
????pwm_ENB.start(0)
????#设置舵机的频率和起始占空比
????pwm_servo = GPIO.PWM(ServoPin, 50)
????pwm_servo.start(0)
????pwm_rled = GPIO.PWM(LED_R, 1000)
????pwm_gled = GPIO.PWM(LED_G, 1000)
????pwm_bled = GPIO.PWM(LED_B, 1000)
????pwm_rled.start(0)
????pwm_gled.start(0)
????pwm_bled.start(0)
????GPIO.setup(EchoPin,GPIO.IN)
????GPIO.setup(TrigPin,GPIO.OUT)
#小车前进
def run():
????GPIO.output(IN1, GPIO.HIGH)
????GPIO.output(IN2, GPIO.LOW)
????GPIO.output(IN3, GPIO.HIGH)
????GPIO.output(IN4, GPIO.LOW)
????pwm_ENA.ChangeDutyCycle(CarSpeedControl)
????pwm_ENB.ChangeDutyCycle(CarSpeedControl)
#小车后退
def back():
????GPIO.output(IN1, GPIO.LOW)
????GPIO.output(IN2, GPIO.HIGH)
????GPIO.output(IN3, GPIO.LOW)
????GPIO.output(IN4, GPIO.HIGH)
????pwm_ENA.ChangeDutyCycle(CarSpeedControl)
????pwm_ENB.ChangeDutyCycle(CarSpeedControl)
#小车左转
def left():
????GPIO.output(IN1, GPIO.LOW)
????GPIO.output(IN2, GPIO.LOW)
????GPIO.output(IN3, GPIO.HIGH)
????GPIO.output(IN4, GPIO.LOW)
????pwm_ENA.ChangeDutyCycle(CarSpeedControl)
????pwm_ENB.ChangeDutyCycle(CarSpeedControl)
#小车右转
def right():
????GPIO.output(IN1, GPIO.HIGH)
????GPIO.output(IN2, GPIO.LOW)
????GPIO.output(IN3, GPIO.LOW)
????GPIO.output(IN4, GPIO.LOW)
????pwm_ENA.ChangeDutyCycle(CarSpeedControl)
????pwm_ENB.ChangeDutyCycle(CarSpeedControl)
#小车原地左转
def spin_left():
????GPIO.output(IN1, GPIO.LOW)
????GPIO.output(IN2, GPIO.HIGH)
????GPIO.output(IN3, GPIO.HIGH)
????GPIO.output(IN4, GPIO.LOW)
????pwm_ENA.ChangeDutyCycle(CarSpeedControl)
????pwm_ENB.ChangeDutyCycle(CarSpeedControl)
#小车原地右转
def spin_right():
????GPIO.output(IN1, GPIO.HIGH)
????GPIO.output(IN2, GPIO.LOW)
????GPIO.output(IN3, GPIO.LOW)
????GPIO.output(IN4, GPIO.HIGH)
????pwm_ENA.ChangeDutyCycle(CarSpeedControl)
????pwm_ENB.ChangeDutyCycle(CarSpeedControl)
#小车沿左前轮前进
def upleft():
????GPIO.output(IN1, GPIO.HIGH)
????GPIO.output(IN2, GPIO.LOW)
????GPIO.output(IN3, GPIO.HIGH)
????GPIO.output(IN4, GPIO.LOW)
????pwm_ENA.ChangeDutyCycle(CarSpeedControl-20)
????pwm_ENB.ChangeDutyCycle(CarSpeedControl+20) ???
#小车沿左后方后退
def downleft():
????GPIO.output(IN1, GPIO.LOW)
????GPIO.output(IN2, GPIO.HIGH)
????GPIO.output(IN3, GPIO.LOW)
????GPIO.output(IN4, GPIO.HIGH)
????pwm_ENA.ChangeDutyCycle(CarSpeedControl-20)
????pwm_ENB.ChangeDutyCycle(CarSpeedControl+20)
#小车沿右上方前进
def upright():
????GPIO.output(IN1, GPIO.HIGH)
????GPIO.output(IN2, GPIO.LOW)
????GPIO.output(IN3, GPIO.HIGH)
????GPIO.output(IN4, GPIO.LOW)
????pwm_ENA.ChangeDutyCycle(CarSpeedControl+20)
????pwm_ENB.ChangeDutyCycle(CarSpeedControl-20) ??
#小车沿右下方后退
def downright():
????GPIO.output(IN1, GPIO.LOW)
????GPIO.output(IN2, GPIO.HIGH)
????GPIO.output(IN3, GPIO.LOW)
????GPIO.output(IN4, GPIO.HIGH)
????pwm_ENA.ChangeDutyCycle(CarSpeedControl+20)
????pwm_ENB.ChangeDutyCycle(CarSpeedControl-20)
#小车停止
def brake():
???GPIO.output(IN1, GPIO.LOW)
???GPIO.output(IN2, GPIO.LOW)
???GPIO.output(IN3, GPIO.LOW)
???GPIO.output(IN4, GPIO.LOW)
???
#小车鸣笛
def whistle():
????GPIO.output(buzzer, GPIO.LOW)
????time.sleep(0.1)
????GPIO.output(buzzer, GPIO.HIGH)
????time.sleep(0.001)
#舵机旋转到指定角度
def servo_appointed_detection(pos):
????for i in range(18):
????????pwm_servo.ChangeDutyCycle(2.5 + 10 * pos/180)
????????time.sleep(0.02) #等待20ms周期结束
????????pwm_servo.ChangeDutyCycle(0) #归零信号
#七彩灯亮指定颜色
def color_led_pwm(iRed,iGreen, iBlue):
????v_red = (100*iRed)/255
????v_green = (100*iGreen)/255
????v_blue = (100*iBlue)/255
????pwm_rled.ChangeDutyCycle(v_red)
????pwm_gled.ChangeDutyCycle(v_green)
????pwm_bled.ChangeDutyCycle(v_blue)
#超声波函数
def Distance_test():#单位cm
????GPIO.output(TrigPin,GPIO.HIGH)
????time.sleep(0.000015)
????GPIO.output(TrigPin,GPIO.LOW)
????while not GPIO.input(EchoPin):
????????pass
????t1 = time.time()
????while GPIO.input(EchoPin):
????????pass
????t2 = time.time()
????print "distance is %d " % (((t2 - t1)* 340 / 2) * 100)
????time.sleep(0.01)
????return ((t2 - t1)* 340 / 2) * 100
#m = 21
#L=[3.75,6,7.5,15,17.5,22.5,25,30,35,35,37.5,45,50,52.5,60,65,67.5,75,80,82.5,90,97.5,98,105,112.5,113,120,127.5,128,135,142.5,143,150,157.5,158,165]#转轴模式
m = 5
L=[15,30,45,60,75,90,105,120,135,150,165]#改用案件
ff=0
target = 20 # 微调的最佳 目标距离
def near_distance(distance):
????i = 0
????if distance > target+1:
????????print("Move up to the right by a large margin and small left down retreat")
????????while int(distance - target) != 0:#一直调整
????h = distance
????right()
????time.sleep(0.5) #偏向角15度左右
????run()
????time.sleep(0.5)
????left()
????time.sleep(0.75)
????back()
????time.sleep(1)
????distance = Distance_test()
????print(str(distance)+" CM")
????
????i = i + 1 ????
????if int(target - distance) > 0 or i>= 10 or h < distance:#防止其它因素导致 出现移动太过情况
????????break
def away_distance(distance):
????i = 0
????if distance < target-1:
????????print("Large left up advance and back down slightly to the right")
????????while int(target - distance) != 0:#一直调整
????h = distance
????left()
????time.sleep(0.5) #偏向角15度左右
????run()
????time.sleep(0.5)
????right()
????time.sleep(0.75)
????back()
????time.sleep(1)
????distance = Distance_test()
????print(str(distance)+" CM")
????
????if int(distance - target) > 0 or i>= 10 or h > distance:#防止其它因素导致 出现移动太过情况
????????break
def pygame_get():
????global m
????global ff
????global CarSpeedControl
????for event_ in pygame.event.get():
????????# 退出事件
????????if event_.type == pygame.QUIT:
????????????done = True
???
# 轴转动事件
????????elif event_.type == pygame.JOYAXISMOTION:
????????????axes = joystick.get_numaxes()
????????????# 获取axis-0 | axis-1轴状态信息
????????????axis0 = joystick.get_axis(0)
????????????axis1 = joystick.get_axis(1)
????axis2 = joystick.get_axis(2)
????axis3 = joystick.get_axis(3)
????axis4 = joystick.get_axis(4)
????for i in range(5):
x="axis"+str(i)
???? print("axis " + str(i) +": " + str(eval(x)))
????if str(axis0) >= "-0.5" and str(axis0)[0] == '-':
left()
????elif str(axis0) >= "0.5" and str(axis0)[0] != '-':
right()
????elif str(axis1) >= "-0.5" and str(axis1)[0] == '-':
run()
????elif str(axis1) >= "0.5" and str(axis1)[0] != '-':
back()
????elif str(axis3) >= "-0.8" and str(axis3)[0] == '-' and ff == 0:
pass #改用按键 防止多次触发命令
????elif str(axis3) >= "0.87" and str(axis3)[0] != '-' and ff == 0:
pass #改用按键 防止多次触发命令
????elif str(axis4) >= "-0.5" and str(axis4)[0] == '-':
GPIO.setup(OutfirePin,GPIO.OUT)
????elif str(axis4) >= "0.5" and str(axis4)[0] != '-':
pass#改用按键 防止多次触发命令(抖动)
????else:
? brake()
GPIO.setup(OutfirePin,GPIO.OUT,initial=GPIO.HIGH)
ff = 0
????????# 方向键改变事件
????????elif event_.type == pygame.JOYHATMOTION:
????????????hats = joystick.get_numhats()
????????????# 获取所有方向键状态信息
????????????for i in range(hats):
????????????????hat = joystick.get_hat(i)
????????????????print(str(hat))
????????????????if str(hat) == "(0, 1)":
????CarSpeedControl=CarSpeedControl+10
????if CarSpeedControl >= 90:
CarSpeedControl=90
????print(CarSpeedControl)
elif str(hat) == "(0, -1)":
????CarSpeedControl=CarSpeedControl-10
????if CarSpeedControl <= 10:
CarSpeedControl=10
????print(CarSpeedControl)
elif str(hat) == "(1, 0)":
????ff = 1
????m=m+1
????if m >= 10:#防止堆栈溢出
m = 10
????servo_appointed_detection(L[m])
????print(m)
elif str(hat) == "(-1, 0)":
????ff = 1
????m=m-1
????if m <= 0:#防止堆栈溢出
m = 0
????servo_appointed_detection(L[m])
????print(m)
else:
????brake()
????ff = 0
# 按键按下或弹起事件(A->超声波微调 B->鸣笛)
????????elif event_.type == pygame.JOYBUTTONDOWN or event_.type == pygame.JOYBUTTONUP:
????????????#buttons = joystick.get_numbuttons()
????????????# 获取所有按键状态信息
????????????#for i in range(buttons):
????????????????#button = joystick.get_button(i)
????????????????#print("button " + str(i) +": " + str(button))
????button0 = joystick.get_button(0)#获取button0(A)按键状态信息
????button1 = joystick.get_button(1)#获取button1(B)按键状态信息
????button2 = joystick.get_button(2)#获取button1(X)按键状态信息
????
????print("button " + "0" +": " + str(button0))
????print("button " + "1" +": " + str(button1))
????print("button " + "2" +": " + str(button2))
????if str(button0) == '1':#微调开始 根据超声波测距 进行水平位移
speed = CarSpeedControl #保存微调前速度
CarSpeedControl = 10#慢速微调
print("Fine tuning starts...")#begin 微调开始 根据超声波测距 进行水平位移
distance = Distance_test()
print(str(distance)+" CM")
????????p = 0
while 1:
????distance = Distance_test()
????p = p + 1
????if p >= 3:#循环调3次
break
????if distance > target+1:
????????near_distance(distance)
continue
????elif distance < target-1:
????????away_distance(distance)
continue
????break
brake()
print("Fine tuning ends.")#end
CarSpeedControl = speed
????elif str(button1) == '1':#鸣笛
GPIO.setup(buzzer,GPIO.OUT,initial=GPIO.LOW)
????else:
GPIO.setup(buzzer,GPIO.OUT,initial=GPIO.HIGH)
????joystick_count = pygame.joystick.get_count()
????return 1
while True:
????#蜂鸣器响连接开始
????GPIO.setup(buzzer,GPIO.OUT,initial=GPIO.LOW)
????time.sleep(1.5)
????GPIO.setup(buzzer,GPIO.OUT,initial=GPIO.HIGH)
????print("15.5 s delay start")
????time.sleep(2.5)
????print("End of hte time delay")
????# 模块初始化
????pygame.init()
????pygame.joystick.init()
????
????
?????
????
????# 手柄对象初始化 # 若只连接了一个手柄，此处带入的参数一般都是0
????try:
joystick = pygame.joystick.Joystick(0)
????except pygame.error:
???? continue
???
????joystick.init()
????
????done = False
????clock = pygame.time.Clock()
????if pygame_get() == 1:
break
try:
????init()
????
????servo_appointed_detection(90)#舵机归位
????while True:
????????global CarSpeedControl ?#to global impact control
global g_Carstate ##to global impact control
????????a=pygame_get()
????????#必要的延时避免过于频繁发送手柄指令 ?
time.sleep(0.05)
except KeyboardInterrupt:
????pass
pwm_ENA.stop()
pwm_ENB.stop()
pwm_rled.stop()
pwm_gled.stop()
pwm_bled.stop()
pwm_servo.stop()
GPIO.cleanup()
????
3.7. ?edgeboardAI 小车驱动实现
3.7.1.功能简介
1.四驱 加速减速实现
连接好接口后，通过向wobot发送如'77 68 06 00 02 0C 01 01'通讯帧，控制各个马达的转向以及转速。
3.7.2.操作截图
该功能的操作截图
3.7.3.代码说明
#前两个是库函数
#serial_port.py
import serial
import time
from threading import Lock
class Serial:
def __init__(self):
portx = "/dev/ttyUSB0"
bps = 115200
self.res = None
self.serial = serial.Serial(portx, int(bps), timeout=1, parity=serial.PARITY_NONE, stopbits=1)
time.sleep(1)
def write(self, data):
lock = Lock()
lock.acquire()
try:
self.serial.write(data)
self.serial.flush()
self.res = self.serial.readline()
finally:
lock.release()
def read(self):
return self.res
serial_connection = Serial()
#cc.py
from serial_port import serial_connection;
from ctypes import *
import time
import serial
import math
comma_head_01_motor = bytes.fromhex('77 68 06 00 02 0C 01 01')
comma_head_02_motor = bytes.fromhex('77 68 06 00 02 0C 01 02')
comma_head_03_motor = bytes.fromhex('77 68 06 00 02 0C 01 03')
comma_head_04_motor = bytes.fromhex('77 68 06 00 02 0C 01 04')
comma_head_21_motor = bytes.fromhex('77 68 06 00 02 0C 02 01')
comma_head_22_motor = bytes.fromhex('77 68 06 00 02 0C 02 02')
comma_head_23_motor = bytes.fromhex('77 68 06 00 02 0C 02 03')
comma_head_24_motor = bytes.fromhex('77 68 06 00 02 0C 02 04')
comma_trail = bytes.fromhex('0A')
class Cart:
????def __init__(self):
????????self.velocity = 25
????????self.Kx=0.85
????????portx = "/dev/ttyUSB0"
????????bps = 115200
????????self.serial = serial.Serial(portx, int(bps), timeout=1, parity=serial.PARITY_NONE, stopbits=1)
????????self.p = 0.8;
????????self.full_speed = self.velocity
????????self.slow_ratio = 0.97;
????????self.min_speed = 20
????def stop(self):
????????self.move([0, 0, 0, 0,0,0,0,0])
????def exchange(self, speed):#if and else
????????if speed > 100:
????????????speed = 100
????????elif speed < -100:
????????????speed = -100
????????else:
????????????speed = speed
????????return speed
????def move(self, speeds):
????????left_front = -int(speeds[0]);
????????right_front = int(speeds[1]);
????????left_rear = -int(speeds[2]);
????????right_rear = int(speeds[3]);
????????
????????m1 = -int(speeds[4]);
????????m2 = int(speeds[5]);
????????m3 = -int(speeds[6]);
????????m4 = int(speeds[7]);
????????self.min_speed = int(min(speeds))
????????
????????left_front=self.exchange(left_front)
????????right_front = self.exchange(right_front)
????????left_rear=self.exchange(left_rear)
????????right_rear = self.exchange(right_rear)
????????
????????send_data_01_motor = comma_head_01_motor + left_front.to_bytes(1, byteorder='big', signed=True) + comma_trail
????????send_data_02_motor = comma_head_02_motor + right_front.to_bytes(1, byteorder='big',signed=True) + comma_trail
????????send_data_03_motor = comma_head_03_motor + left_rear.to_bytes(1, byteorder='big', signed=True) + comma_trail
????????send_data_04_motor = comma_head_04_motor + right_rear.to_bytes(1, byteorder='big', signed=True) + comma_trail
????????send_data_21_motor = comma_head_21_motor + m1.to_bytes(1, byteorder='big', signed=True) + comma_trail
????????send_data_22_motor = comma_head_22_motor + m2.to_bytes(1, byteorder='big',signed=True) + comma_trail
????????send_data_23_motor = comma_head_23_motor + m3.to_bytes(1, byteorder='big', signed=True) + comma_trail
????????send_data_24_motor = comma_head_24_motor + m4.to_bytes(1, byteorder='big', signed=True) + comma_trail
????????
????????self.serial.write(send_data_01_motor)
????????self.serial.write(send_data_02_motor)
????????self.serial.write(send_data_03_motor)
????????self.serial.write(send_data_04_motor)
????????
????????self.serial.write(send_data_21_motor)
????????self.serial.write(send_data_22_motor)
????????self.serial.write(send_data_23_motor)
????????self.serial.write(send_data_24_motor)
????default_speed = 20
????def right(self,CarSpeedControl=default_speed):
????????self.move([-CarSpeedControl,0,-CarSpeedControl,0,0,0,0,0])
????????
????def left(self,CarSpeedControl=default_speed):
????????self.move([0,-CarSpeedControl,0,-CarSpeedControl,0,0,0,0])
????????
????def run(self,CarSpeedControl=default_speed):
????????self.move([-CarSpeedControl,-CarSpeedControl,-CarSpeedControl,-CarSpeedControl,0,0,0,0])
????????
????def back(self,CarSpeedControl=default_speed):
????????self.move([CarSpeedControl,CarSpeedControl,CarSpeedControl,CarSpeedControl,0,0,0,0])
????
????
????def Drop_ball_program(self):
????????#Drop ball program and keep
????
????????self.move([0,0,0,0,0,10,0,0])
????????time.sleep(1.5)#1~1.7
????????self.move([0,0,0,0,0,-40,0,0])
????????time.sleep(2)
????????
????def Display_the_flag(self):
????????#Display the flag
????????#self.move([0,0,0,0,30,0,0,0])
????????#time.sleep(1.25)
????????#self.move([0,0,0,0,0,0,0,0])
????????#time.sleep(5)
????????
????????self.move([0,0,0,0,10,0,0,0])
????????time.sleep(10)
????????self.move([0,0,0,0,0,0,0,0])
????????
????def brake(self,CarSpeedControl=0):
????????self.move([0,0,0,0,0,0,0,0])
????????
cart = Cart()
if __name__ == "__main__":
pass
#test.py
import time
import struct
from serial_port import serial_connection
from cc import cart;
serial = serial_connection
??
class Servo:
????def __init__(self, ID):
????????self.ID = ID
????????self.ID_str = '{:02x}'.format(ID)
????def servocontrol(self, angle, speed):
????????cmd_servo_data = bytes.fromhex('77 68 06 00 02 36') + bytes.fromhex(self.ID_str) + speed.to_bytes(1,
????????????????????????????????????????????????????????????????????????????????????????????????????????????byteorder='big', \
????????????????????????????????????????????????????????????????????????????????????????????????????????????signed=True) + angle.to_bytes(
????????????1, byteorder='big', signed=True) + bytes.fromhex('0A')
????????# for i in range(0,2):
????????serial.write(cmd_servo_data)
????????# ????time.sleep(0.3)
class Servo_pwm:
????def __init__(self, ID):
????????self.ID = ID
????????self.ID_str = '{:02x}'.format(ID)
????def servocontrol(self, angle, speed):
????????cmd_servo_data = bytes.fromhex('77 68 06 00 02 0B') + bytes.fromhex(self.ID_str) + speed.to_bytes(1,
????????????????????????????????????????????????????????????????????????????????????????????????????????????byteorder='big', \
????????????????????????????????????????????????????????????????????????????????????????????????????????????signed=True) + angle.to_bytes(
????????????1, byteorder='big', signed=False) + bytes.fromhex('0A')
????????# for i in range(0,2):
????????serial.write(cmd_servo_data)
????????# time.sleep(0.3)
CarSpeedControl = 20 #init speed
c=cart
def control(x):
????global CarSpeedControl
????if str(x) == 'a' or str(x) == 'A':#Left
????????c.left(CarSpeedControl)
????????
????elif str(x) == 'd' or str(x) == 'D':#Right
????????c.right(CarSpeedControl)
????????
????elif str(x) == 'w' or str(x) == 'W':#Run
????????c.run(CarSpeedControl)
????????
????elif str(x) == 's' or str(x) == 'S':#Back
????????c.back(CarSpeedControl)
????????
????elif str(x) == 'j' or str(x) == 'J':#Accelerate
????????CarSpeedControl = CarSpeedControl + 10
????????if CarSpeedControl > 90:#防止堆栈溢出
????????????CarSpeedControl = 90
????????????
????elif str(x) == 'k' or str(x) == 'K':#Slow down
????????CarSpeedControl = CarSpeedControl - 10
????????if CarSpeedControl < 10:#防止堆栈溢出
????????????CarSpeedControl = 10
????
????elif str(x) == 'T' or str(x) == 't':#Display_the_flag
????????c.Display_the_flag()
????????????
????elif str(x) == 'Y' or str(x) == 'y':#Drop_ball_program
????????c.Drop_ball_program()
????else:
????????c.brake()
????????
#from Serial_0305 import SerialAssistant
if __name__ == '__main__':
????while 1:
????????scan = input("Key command: ")
????????if str(scan) == 'p' or str(scan) == 'P':
????????????break
????????control(scan)
????
????pass
????
????
3.8. ???基于edgeboardAI和WOBOT超声波微调
3.8.1.功能简介
1.在驱动的基础上实现调整最佳距离
连接好接口后，通过向wobot发送如'77 68 06 00 02 0C 01 01'通讯帧，控制各个马达的转向以及转速。在着基础上，运用转向和速度的测试调节，实现小车的水平移动，直到调整到设定好的最佳位置。
3.8.2.操作截图
该功能的操作截图
3.8.3.代码说明
#前两个是库函数
#serial_port.py
import serial
import time
from threading import Lock
class Serial:
def __init__(self):
portx = "/dev/ttyUSB0"
bps = 115200
self.res = None
self.serial = serial.Serial(portx, int(bps), timeout=1, parity=serial.PARITY_NONE, stopbits=1)
time.sleep(1)
def write(self, data):
lock = Lock()
lock.acquire()
try:
self.serial.write(data)
self.serial.flush()
self.res = self.serial.readline()
finally:
lock.release()
def read(self):
return self.res
serial_connection = Serial()
#cc.py
from serial_port import serial_connection;
from ctypes import *
import time
import serial
import math
comma_head_01_motor = bytes.fromhex('77 68 06 00 02 0C 01 01')
comma_head_02_motor = bytes.fromhex('77 68 06 00 02 0C 01 02')
comma_head_03_motor = bytes.fromhex('77 68 06 00 02 0C 01 03')
comma_head_04_motor = bytes.fromhex('77 68 06 00 02 0C 01 04')
comma_head_21_motor = bytes.fromhex('77 68 06 00 02 0C 02 01')
comma_head_22_motor = bytes.fromhex('77 68 06 00 02 0C 02 02')
comma_head_23_motor = bytes.fromhex('77 68 06 00 02 0C 02 03')
comma_head_24_motor = bytes.fromhex('77 68 06 00 02 0C 02 04')
comma_trail = bytes.fromhex('0A')
class Cart:
????def __init__(self):
????????self.velocity = 25
????????self.Kx=0.85
????????portx = "/dev/ttyUSB0"
????????bps = 115200
????????self.serial = serial.Serial(portx, int(bps), timeout=1, parity=serial.PARITY_NONE, stopbits=1)
????????self.p = 0.8;
????????self.full_speed = self.velocity
????????self.slow_ratio = 0.97;
????????self.min_speed = 20
????def stop(self):
????????self.move([0, 0, 0, 0,0,0,0,0])
????def exchange(self, speed):#if and else
????????if speed > 100:
????????????speed = 100
????????elif speed < -100:
????????????speed = -100
????????else:
????????????speed = speed
????????return speed
????def move(self, speeds):
????????left_front = -int(speeds[0]);
????????right_front = int(speeds[1]);
????????left_rear = -int(speeds[2]);
????????right_rear = int(speeds[3]);
????????
????????m1 = -int(speeds[4]);
????????m2 = int(speeds[5]);
????????m3 = -int(speeds[6]);
????????m4 = int(speeds[7]);
????????self.min_speed = int(min(speeds))
????????
????????left_front=self.exchange(left_front)
????????right_front = self.exchange(right_front)
????????left_rear=self.exchange(left_rear)
????????right_rear = self.exchange(right_rear)
????????
????????send_data_01_motor = comma_head_01_motor + left_front.to_bytes(1, byteorder='big', signed=True) + comma_trail
????????send_data_02_motor = comma_head_02_motor + right_front.to_bytes(1, byteorder='big',signed=True) + comma_trail
????????send_data_03_motor = comma_head_03_motor + left_rear.to_bytes(1, byteorder='big', signed=True) + comma_trail
????????send_data_04_motor = comma_head_04_motor + right_rear.to_bytes(1, byteorder='big', signed=True) + comma_trail
????????send_data_21_motor = comma_head_21_motor + m1.to_bytes(1, byteorder='big', signed=True) + comma_trail
????????send_data_22_motor = comma_head_22_motor + m2.to_bytes(1, byteorder='big',signed=True) + comma_trail
????????send_data_23_motor = comma_head_23_motor + m3.to_bytes(1, byteorder='big', signed=True) + comma_trail
????????send_data_24_motor = comma_head_24_motor + m4.to_bytes(1, byteorder='big', signed=True) + comma_trail
????????
????????self.serial.write(send_data_01_motor)
????????self.serial.write(send_data_02_motor)
????????self.serial.write(send_data_03_motor)
????????self.serial.write(send_data_04_motor)
????????
????????self.serial.write(send_data_21_motor)
????????self.serial.write(send_data_22_motor)
????????self.serial.write(send_data_23_motor)
????????self.serial.write(send_data_24_motor)
????default_speed = 20
????def right(self,CarSpeedControl=default_speed):
????????self.move([-CarSpeedControl,0,-CarSpeedControl,0,0,0,0,0])
????????
????def left(self,CarSpeedControl=default_speed):
????????self.move([0,-CarSpeedControl,0,-CarSpeedControl,0,0,0,0])
????????
????def run(self,CarSpeedControl=default_speed):
????????self.move([-CarSpeedControl,-CarSpeedControl,-CarSpeedControl,-CarSpeedControl,0,0,0,0])
????????
????def back(self,CarSpeedControl=default_speed):
????????self.move([CarSpeedControl,CarSpeedControl,CarSpeedControl,CarSpeedControl,0,0,0,0])
????
????
????def Drop_ball_program(self):
????????#Drop ball program and keep
????
????????self.move([0,0,0,0,0,10,0,0])
????????time.sleep(1.5)#1~1.7
????????self.move([0,0,0,0,0,-40,0,0])
????????time.sleep(2)
????????
????def Display_the_flag(self):
????????#Display the flag
????????#self.move([0,0,0,0,30,0,0,0])
????????#time.sleep(1.25)
????????#self.move([0,0,0,0,0,0,0,0])
????????#time.sleep(5)
????????
????????self.move([0,0,0,0,10,0,0,0])
????????time.sleep(10)
????????self.move([0,0,0,0,0,0,0,0])
????????
????def brake(self,CarSpeedControl=0):
????????self.move([0,0,0,0,0,0,0,0])
????????
cart = Cart()
if __name__ == "__main__":
pass
#test.py
import time
import struct
from serial_port import serial_connection
from cc import cart;
serial = serial_connection
??
class UltrasonicSensor():
????def __init__(self, port):
????????self.port = port
????????port_str = '{:02x}'.format(port)
????????self.cmd_data = bytes.fromhex('77 68 04 00 01 D1 {} 0A'.format(port_str))
????def read(self):
????????serial.write(self.cmd_data)
????????print(self.cmd_data)
????????time.sleep(0.01)
????????return_data = serial.read()
????????print(return_data)
????????if len(return_data)<11 or return_data[7] != 0xD1 or return_data[8] != self.port:
????????????return None
????????#print(return_data.hex())
????????return_data_ultrasonic = return_data[3:7]
????????ultrasonic_sensor = struct.unpack('<f', struct.pack('4B', *(return_data_ultrasonic)))[0]
????????print(ultrasonic_sensor)
????????return_data = ''
????????return int(ultrasonic_sensor)
class Servo:
????def __init__(self, ID):
????????self.ID = ID
????????self.ID_str = '{:02x}'.format(ID)
????def servocontrol(self, angle, speed):
????????cmd_servo_data = bytes.fromhex('77 68 06 00 02 36') + bytes.fromhex(self.ID_str) + speed.to_bytes(1,
????????????????????????????????????????????????????????????????????????????????????????????????????????????byteorder='big', \
????????????????????????????????????????????????????????????????????????????????????????????????????????????signed=True) + angle.to_bytes(
????????????1, byteorder='big', signed=True) + bytes.fromhex('0A')
????????# for i in range(0,2):
????????serial.write(cmd_servo_data)
????????# ????time.sleep(0.3)
class Servo_pwm:
????def __init__(self, ID):
????????self.ID = ID
????????self.ID_str = '{:02x}'.format(ID)
????def servocontrol(self, angle, speed):
????????cmd_servo_data = bytes.fromhex('77 68 06 00 02 0B') + bytes.fromhex(self.ID_str) + speed.to_bytes(1,
????????????????????????????????????????????????????????????????????????????????????????????????????????????byteorder='big', \
????????????????????????????????????????????????????????????????????????????????????????????????????????????signed=True) + angle.to_bytes(
????????????1, byteorder='big', signed=False) + bytes.fromhex('0A')
????????# for i in range(0,2):
????????serial.write(cmd_servo_data)
????????# time.sleep(0.3)
class Buzzer:
????def __init__(self):
????????self.cmd_data = bytes.fromhex('77 68 06 00 02 3D 03 02 0A')
????def rings(self):
????????serial.write(self.cmd_data)
class Magneto_sensor:
????def __init__(self,port):
????????self.port=port
????????port_str = '{:02x}'.format(self.port)
????????self.cmd_data = bytes.fromhex('77 68 04 00 01 CF {} 0A'.format(port_str))
????def read(self):
????????serial.write(self.cmd_data)
????????return_data = serial.read()
????????# print("return_data=",return_data[8])
????????if len(return_data) < 11 or return_data[7] != 0xCF or return_data[8] != self.port:
????????????return None
????????# print(return_data.hex())
????????return_data = return_data[3:7]
????????mag_sensor = struct.unpack('<i', struct.pack('4B', *(return_data)))[0]
????????# print(ultrasonic_sensor)
????????return int(mag_sensor)
class Test:
????pass
CarSpeedControl = 20 #init speed
c=cart
target = 30 # 微调的最佳 目标距离
def Distance_test():
????ultr_sensor=UltrasonicSensor(1)
????distance=ultr_sensor.read()
????return distance
def near_distance(distance):
????i = 0#Limit of adjustment times
????if distance > target+1:
????????print("Move up to the right by a large margin and small left down retreat")
????????while int(distance - target) != 0:#一直调整
????????????h = distance
????????????c.right()
????????????time.sleep(0.5) #偏向角15度左右
????????????c.run()
????????????time.sleep(0.5)
????????????c.left()
????????????time.sleep(0.75)
????????????c.back()
????????????time.sleep(0.5)
????????????c.brake()
????????????
????????????
????????????i = i + 1 ????
????????????if i>= 5 or h < distance:#防止其它因素导致 出现移动太过情况
????????????????break
def away_distance(distance):
????i = 0#Limit of adjustment times
????if distance < target-1:
????????print("Large left up advance and back down slightly to the right")
????????while int(target - distance) != 0:#一直调整
????????????h = distance
????????????c.left()
????????????time.sleep(0.5) #偏向角15度左右
????????????c.run()
????????????time.sleep(0.5)
????????????c.right()
????????????time.sleep(0.75)
????????????c.back()
????????????time.sleep(0.5)
????????????c.brake()
????????????
????????????
????????????i = i + 1 ????
????????????if i>= 5 or h > distance:#防止其它因素导致 出现移动太过情况
????????????????break
def control(x):
????global CarSpeedControl
????if str(x) == 'a' or str(x) == 'A':#Left
????????c.left(CarSpeedControl)
????????
????elif str(x) == 'd' or str(x) == 'D':#Right
????????c.right(CarSpeedControl)
????????
????elif str(x) == 'w' or str(x) == 'W':#Run
????????c.run(CarSpeedControl)
????????
????elif str(x) == 's' or str(x) == 'S':#Back
????????c.back(CarSpeedControl)
????????
????elif str(x) == 'j' or str(x) == 'J':#Accelerate
????????CarSpeedControl = CarSpeedControl + 10
????????if CarSpeedControl > 90:#防止堆栈溢出
????????????CarSpeedControl = 90
????????????
????elif str(x) == 'k' or str(x) == 'K':#Slow down
????????CarSpeedControl = CarSpeedControl - 10
????????if CarSpeedControl < 10:#防止堆栈溢出
????????????CarSpeedControl = 10
????
????elif str(x) == 'T' or str(x) == 't':#Display_the_flag
????????c.Display_the_flag()
????????????
????elif str(x) == 'Y' or str(x) == 'y':#Drop_ball_program
????????c.Drop_ball_program()
?????????
????elif str(x) == 'U' or str(x) == 'u':
????????speed = CarSpeedControl #保存微调前速度
????????CarSpeedControl = 10#慢速微调
????????print("Fine tuning starts...")#begin 微调开始 根据超声波测距 进行水平位移
????????p = 0
????????distance = Distance_test()
????????print(distance)
????????while 1:
????????????p = p + 1
????????????if p >= 2:#循环调3次
????????????????break
????????????
????????????time.sleep(1)
????????????if distance > target+1:
????????????????near_distance(distance)
????????????????
????????????????ultr_sensor1=UltrasonicSensor(1)
????????????????distance1=ultr_sensor1.read()
????????????????print(distance1)
????????????????
????????????????continue
????????????elif distance < target-1:
????????????????away_distance(distance)
????????????????
????????????????ultr_sensor1=UltrasonicSensor(1)
????????????????distance1=ultr_sensor1.read()
????????????????print(distance1)
????????????????continue
????????????break
????????c.brake()
????????
????????print("Fine tuning ends.")#end
????????CarSpeedControl = speed
????else:
????????c.brake()
????????
#from Serial_0305 import SerialAssistant
if __name__ == '__main__':
????while 1:
????????scan = input("Key command: ")
????????if str(scan) == 'p' or str(scan) == 'P':
????????????break
????????control(scan)
????
????pass
3.9. ?edgeboardAI 小车展旗，放粮架主要代码
3.9.1.功能简介
1.展旗帜，放粮架
wobot 通讯控制电机 双板 1，2 帧结构识别
同步模式
77 68 06 00 02 0C 01 01 50 0A
异步模式
1电机板（m1 ~ m4）
77 68 06 00 02 0C 01 01 50 0A
2电机板
77 68 06 00 02 0C 01 02 50 0A
这里要启用异步模式，1板控制驱动，2板来实现展旗帜和放粮架。
3.9.2.操作截图
图一展旗帜，图二粮架
3.9.3.代码说明
#在之前两个库的基础上
#move函数
send_data_01_motor = comma_head_01_motor + left_front.to_bytes(1, byteorder='big', signed=True) + comma_trail
send_data_02_motor = comma_head_02_motor + right_front.to_bytes(1, byteorder='big',signed=True) + comma_trail
send_data_03_motor = comma_head_03_motor + left_rear.to_bytes(1, byteorder='big', signed=True) + comma_trail
send_data_04_motor = comma_head_04_motor + right_rear.to_bytes(1, byteorder='big', signed=True) + comma_trail
send_data_21_motor = comma_head_21_motor + m1.to_bytes(1, byteorder='big', signed=True) + comma_trail
send_data_22_motor = comma_head_22_motor + m2.to_bytes(1, byteorder='big',signed=True) + comma_trail
send_data_23_motor = comma_head_23_motor + m3.to_bytes(1, byteorder='big', signed=True) + comma_trail
send_data_24_motor = comma_head_24_motor + m4.to_bytes(1, byteorder='big', signed=True) + comma_trail
self.serial.write(send_data_01_motor)
self.serial.write(send_data_02_motor)
self.serial.write(send_data_03_motor)
self.serial.write(send_data_04_motor)
self.serial.write(send_data_21_motor)
self.serial.write(send_data_22_motor)
self.serial.write(send_data_23_motor)
self.serial.write(send_data_24_motor)
#Drop ball program
'''
c.move([0,0,0,0,0,10,0,0])
time.sleep(1.5)#1~1.7
c.move([0,0,0,0,0,-40,0,0])
time.sleep(2)
'''
#Display the flag
'''
c.move([0,0,0,0,30,0,0,0])
time.sleep(1.25)
c.move([0,0,0,0,0,0,0,0])
time.sleep(5)
c.move([0,0,0,0,30,0,0,0])
time.sleep(1.25)
c.move([0,0,0,0,0,0,0,0])
'''
3.10. ?基于多个舵机的抓机的实现，以及1，2两块电机板驱动触笔的实现
3.10.1.功能简介
1.基于多个舵机的抓机实现
wobot 通讯控制电机 双板 1，2 帧结构识别
同步模式
77 68 06 00 02 0C 01 01 50 0A
异步模式
1电机板（m1 ~ m4）
77 68 06 00 02 0C 01 01 50 0A
2电机板
77 68 06 00 02 0C 01 02 50 0A
2.这里要启用异步模式，1板控制驱动，2板来实现展旗帜和放粮架。
.基于1，2两块电机板驱动对于上层AI的调用接口实现
3.触笔的实现
点触笔使用马达和齿轮等传动装置控制
3.10.2.操作截图
3.10.3.代码说明
#1
#cc.py
from serial_port import serial_connection;
from ctypes import *
import time
import serial
import math
comma_head_01_motor = bytes.fromhex('77 68 06 00 02 0C 01 01')
comma_head_02_motor = bytes.fromhex('77 68 06 00 02 0C 01 02')
comma_head_03_motor = bytes.fromhex('77 68 06 00 02 0C 01 03')
comma_head_04_motor = bytes.fromhex('77 68 06 00 02 0C 01 04')
comma_head_21_motor = bytes.fromhex('77 68 06 00 02 0C 02 01')
comma_head_22_motor = bytes.fromhex('77 68 06 00 02 0C 02 02')
comma_head_23_motor = bytes.fromhex('77 68 06 00 02 0C 02 03')
comma_head_24_motor = bytes.fromhex('77 68 06 00 02 0C 02 04')
comma_trail = bytes.fromhex('0A')
class Cart:
????def __init__(self):
????????self.velocity = 25
????????self.Kx=0.85
????????portx = "/dev/ttyUSB0"
????????bps = 115200
????????self.serial = serial.Serial(portx, int(bps), timeout=1, parity=serial.PARITY_NONE, stopbits=1)
????????self.p = 0.8;
????????self.full_speed = self.velocity
????????self.slow_ratio = 0.97;
????????self.min_speed = 20
????def stop(self):
????????self.move([0, 0, 0, 0,0,0,0,0])
????def exchange(self, speed):#if and else
????????if speed > 100:
????????????speed = 100
????????elif speed < -100:
????????????speed = -100
????????else:
????????????speed = speed
????????return speed
????def move(self, speeds):
????????left_front = -int(speeds[0]);
????????right_front = int(speeds[1]);
????????left_rear = -int(speeds[2]);
????????right_rear = int(speeds[3]);
????????
????????m1 = -int(speeds[4]);
????????m2 = int(speeds[5]);
????????m3 = -int(speeds[6]);
????????m4 = int(speeds[7]);
????????self.min_speed = int(min(speeds))
????????
????????left_front=self.exchange(left_front)
????????right_front = self.exchange(right_front)
????????left_rear=self.exchange(left_rear)
????????right_rear = self.exchange(right_rear)
????????
????????send_data_01_motor = comma_head_01_motor + left_front.to_bytes(1, byteorder='big', signed=True) + comma_trail
????????send_data_02_motor = comma_head_02_motor + right_front.to_bytes(1, byteorder='big',signed=True) + comma_trail
????????send_data_03_motor = comma_head_03_motor + left_rear.to_bytes(1, byteorder='big', signed=True) + comma_trail
????????send_data_04_motor = comma_head_04_motor + right_rear.to_bytes(1, byteorder='big', signed=True) + comma_trail
????????send_data_21_motor = comma_head_21_motor + m1.to_bytes(1, byteorder='big', signed=True) + comma_trail
????????send_data_22_motor = comma_head_22_motor + m2.to_bytes(1, byteorder='big',signed=True) + comma_trail
????????send_data_23_motor = comma_head_23_motor + m3.to_bytes(1, byteorder='big', signed=True) + comma_trail
????????send_data_24_motor = comma_head_24_motor + m4.to_bytes(1, byteorder='big', signed=True) + comma_trail
????????
????????self.serial.write(send_data_01_motor)
????????self.serial.write(send_data_02_motor)
????????self.serial.write(send_data_03_motor)
????????self.serial.write(send_data_04_motor)
????????
????????self.serial.write(send_data_21_motor)
????????self.serial.write(send_data_22_motor)
????????self.serial.write(send_data_23_motor)
????????self.serial.write(send_data_24_motor)
????default_speed = 20
????def left(self,CarSpeedControl=default_speed):
????????self.move([CarSpeedControl,0,CarSpeedControl,0,0,0,0,0])
????????
????def right(self,CarSpeedControl=default_speed):
????????self.move([0,CarSpeedControl,0,CarSpeedControl,0,0,0,0])
????????
????def run(self,CarSpeedControl=default_speed):
????????self.move([CarSpeedControl,CarSpeedControl,CarSpeedControl,CarSpeedControl,0,0,0,0])
????????
????def back(self,CarSpeedControl=default_speed):
????????self.move([-CarSpeedControl,-CarSpeedControl,-CarSpeedControl,-CarSpeedControl,0,0,0,0])
????
????def Pen_run(self,speed):
????????self.move([0,0,0,0,0,0,speed,0])
????????time.sleep(1)
????????self.move([0,0,0,0,0,0,0,0])
????def Drop_ball_program(self):
????????#Drop ball program and keep
????
????????self.move([0,0,0,0,0,10,0,0])
????????time.sleep(1.5)#1~1.7
????????self.move([0,0,0,0,0,-40,0,0])
????????time.sleep(2)
????????
????def Display_the_flag(self):
????????#Display the flag
????????self.move([0,0,0,0,30,0,0,0])
????????time.sleep(1.25)
????????self.move([0,0,0,0,0,0,0,0])
????????time.sleep(5)
????????
????????#self.move([0,0,0,0,10,0,0,0])
????????#time.sleep(2)
????????#self.move([0,0,0,0,0,0,0,0])
????????
????def brake(self,CarSpeedControl=0):
????????self.move([0,0,0,0,0,0,0,0])
????????
cart = Cart()
if __name__ == "__main__":
????pass
#2
import time
import struct
from serial_port import serial_connection
serial = serial_connection
class Servo:
????def __init__(self, ID):
????????self.ID = ID
????????self.ID_str = '{:02x}'.format(ID)
????def servocontrol(self, angle, speed):
????????cmd_servo_data = bytes.fromhex('77 68 06 00 02 36') + bytes.fromhex(self.ID_str) + speed.to_bytes(1,
????????????????????????????????????????????????????????????????????????????????????????????????????????????byteorder='big', \
????????????????????????????????????????????????????????????????????????????????????????????????????????????signed=True) + angle.to_bytes(
????????????1, byteorder='big', signed=True) + bytes.fromhex('0A')
????????# for i in range(0,2):
????????serial.write(cmd_servo_data)
????????# ????time.sleep(0.3)
class Servo_pwm:
????def __init__(self, ID):
????????self.ID = ID
????????self.ID_str = '{:02x}'.format(ID)
????def servocontrol(self, angle, speed):
????????cmd_servo_data = bytes.fromhex('77 68 06 00 02 0B') + bytes.fromhex(self.ID_str) + speed.to_bytes(1,
????????????????????????????????????????????????????????????????????????????????????????????????????????????byteorder='big', \
????????????????????????????????????????????????????????????????????????????????????????????????????????????signed=True) + angle.to_bytes(
????????????1, byteorder='big', signed=False) + bytes.fromhex('0A')
????????# for i in range(0,2):
????????serial.write(cmd_servo_data)
????????# time.sleep(0.3)
#from Serial_0305 import SerialAssistant
if __name__ == '__main__':
????servo1=Servo(2)
????servo2=Servo_pwm(2)
????#servo3 = Servo(1)
????
????i = 0
????
????servo2.servocontrol(50,100)
????print("cccccc")
????'''
????while i <= 8:
????????i = i+1
????????print(i)
????????servo1.servocontrol(-10, 100)
????????print("5,100")
????????time.sleep(3)
????????
????????servo1.servocontrol(-85, 100)
????????print("-85,100")
????????time.sleep(3)
????'''
????servo1.servocontrol(-10, 100)
????print("-10,100")
????time.sleep(3)
????
????servo2.servocontrol(0,100)
????print("cccccc")
????time.sleep(3)
????
????servo1.servocontrol(-85, 100)
????print("-85,100")
????time.sleep(3)
3.11. ?蓝牙实现 wobot小车通信控制
3.11.1.功能简介
1.基于多个舵机的抓机实现
wobot 通讯控制电机 双板 1，2 帧结构识别
同步模式
77 68 06 00 02 0C 01 01 50 0A
异步模式
1电机板（m1 ~ m4）
77 68 06 00 02 0C 01 01 50 0A
2电机板
77 68 06 00 02 0C 01 02 50 0A
2.这里要启用异步模式，1板控制驱动，2板来实现展旗帜和放粮架。
.基于1，2两块电机板驱动对于上层AI的调用接口实现
3插入远程USB蓝牙接口到edgeboardAI接口，连接上蓝牙
通过接受蓝牙的命令，实现对应的控制
3.11.2.操作截图
3.11.3.代码说明
#cc.py
from serial_port import serial_connection;
from ctypes import *
import time
import serial
import math
comma_head_01_motor = bytes.fromhex('77 68 06 00 02 0C 01 01')
comma_head_02_motor = bytes.fromhex('77 68 06 00 02 0C 01 02')
comma_head_03_motor = bytes.fromhex('77 68 06 00 02 0C 01 03')
comma_head_04_motor = bytes.fromhex('77 68 06 00 02 0C 01 04')
comma_head_21_motor = bytes.fromhex('77 68 06 00 02 0C 02 01')
comma_head_22_motor = bytes.fromhex('77 68 06 00 02 0C 02 02')
comma_head_23_motor = bytes.fromhex('77 68 06 00 02 0C 02 03')
comma_head_24_motor = bytes.fromhex('77 68 06 00 02 0C 02 04')
comma_trail = bytes.fromhex('0A')
class Cart:
????def __init__(self):
????????self.velocity = 25
????????self.Kx=0.85
????????portx = "/dev/ttyUSB0"
????????bps = 115200
????????self.serial = serial.Serial(portx, int(bps), timeout=1, parity=serial.PARITY_NONE, stopbits=1)
????????self.p = 0.8;
????????self.full_speed = self.velocity
????????self.slow_ratio = 0.97;
????????self.min_speed = 20
????def stop(self):
????????self.move([0, 0, 0, 0,0,0,0,0])
????def exchange(self, speed):#if and else
????????if speed > 100:
????????????speed = 100
????????elif speed < -100:
????????????speed = -100
????????else:
????????????speed = speed
????????return speed
????def move(self, speeds):
????????left_front = -int(speeds[0]);
????????right_front = int(speeds[1]);
????????left_rear = -int(speeds[2]);
????????right_rear = int(speeds[3]);
????????
????????m1 = -int(speeds[4]);
????????m2 = int(speeds[5]);
????????m3 = -int(speeds[6]);
????????m4 = int(speeds[7]);
????????self.min_speed = int(min(speeds))
????????
????????left_front=self.exchange(left_front)
????????right_front = self.exchange(right_front)
????????left_rear=self.exchange(left_rear)
????????right_rear = self.exchange(right_rear)
????????
????????send_data_01_motor = comma_head_01_motor + left_front.to_bytes(1, byteorder='big', signed=True) + comma_trail
????????send_data_02_motor = comma_head_02_motor + right_front.to_bytes(1, byteorder='big',signed=True) + comma_trail
????????send_data_03_motor = comma_head_03_motor + left_rear.to_bytes(1, byteorder='big', signed=True) + comma_trail
????????send_data_04_motor = comma_head_04_motor + right_rear.to_bytes(1, byteorder='big', signed=True) + comma_trail
????????send_data_21_motor = comma_head_21_motor + m1.to_bytes(1, byteorder='big', signed=True) + comma_trail
????????send_data_22_motor = comma_head_22_motor + m2.to_bytes(1, byteorder='big',signed=True) + comma_trail
????????send_data_23_motor = comma_head_23_motor + m3.to_bytes(1, byteorder='big', signed=True) + comma_trail
????????send_data_24_motor = comma_head_24_motor + m4.to_bytes(1, byteorder='big', signed=True) + comma_trail
????????
????????self.serial.write(send_data_01_motor)
????????self.serial.write(send_data_02_motor)
????????self.serial.write(send_data_03_motor)
????????self.serial.write(send_data_04_motor)
????????
????????self.serial.write(send_data_21_motor)
????????self.serial.write(send_data_22_motor)
????????self.serial.write(send_data_23_motor)
????????self.serial.write(send_data_24_motor)
????default_speed = 20
????def left(self,CarSpeedControl=default_speed):
????????self.move([CarSpeedControl,0,CarSpeedControl,0,0,0,0,0])
????????
????def right(self,CarSpeedControl=default_speed):
????????self.move([0,CarSpeedControl,0,CarSpeedControl,0,0,0,0])
????????
????def run(self,CarSpeedControl=default_speed):
????????self.move([CarSpeedControl,CarSpeedControl,CarSpeedControl,CarSpeedControl,0,0,0,0])
????????
????def back(self,CarSpeedControl=default_speed):
????????self.move([-CarSpeedControl,-CarSpeedControl,-CarSpeedControl,-CarSpeedControl,0,0,0,0])
????
????def Pen_run(self,speed):
????????self.move([0,0,0,0,0,0,speed,0])
????????time.sleep(1)
????????self.move([0,0,0,0,0,0,0,0])
????def Drop_ball_program(self):
????????#Drop ball program and keep
????
????????self.move([0,0,0,0,0,10,0,0])
????????time.sleep(1.5)#1~1.7
????????self.move([0,0,0,0,0,-40,0,0])
????????time.sleep(2)
????????
????def Display_the_flag(self):
????????#Display the flag
????????self.move([0,0,0,0,30,0,0,0])
????????time.sleep(1.25)
????????self.move([0,0,0,0,0,0,0,0])
????????time.sleep(5)
????????
????????#self.move([0,0,0,0,10,0,0,0])
????????#time.sleep(2)
????????#self.move([0,0,0,0,0,0,0,0])
????????
????def brake(self,CarSpeedControl=0):
????????self.move([0,0,0,0,0,0,0,0])
????????
cart = Cart()
if __name__ == "__main__":
????pass
#2
from joystick import JoyStick
from cc import cart
import time
import cv2
import threading
import json
import config
#本文件用作无人驾驶车道数据采集
c = cart
class Logger:
????def __init__(self):
????????self.camera = cv2.VideoCapture(config.front_cam)
????????self.started = False
????????self.stopped_ = False
????????self.counter = 0
????????self.map = {}
????????self.result_dir = "train"
????def start(self):
????????self.started = True
????????cart.steer(0)
????????pass
????def stop(self):
????????if self.stopped_:
????????????return
????????self.stopped_ = True
????????cart.stop()
????????path = "{}/result.json".format(self.result_dir)
????????with open(path, 'w') as fp:
????????????json.dump(self.map.copy(), fp)
????????pass
????def log(self, axis):
????????if self.started :
????????????print("axis:".format(axis))
????????????cart.steer(axis)
????????????return_value, image = self.camera.read()
????????????path = "{}/{}.jpg".format(self.result_dir, self.counter)
????????????self.map[self.counter] = axis
????????????cv2.imwrite(path, image)
????????????self.counter = self.counter + 1
????????????
????def stopped(self):
????????return self.stopped_
js = JoyStick()
logger = Logger()
def joystick_thread():
????js.open()
????i = 20
????while not logger.stopped():
????????time, value, type_, number = js.read()
????????if js.type(type_) == "button":
????????????print("button:{} state: {}".format(number, value))
????????????if number == 6 and value == 1:
????????????????logger.start()
????????????if number == 7 and value == 1:
????????????????logger.stop()
????????????
????????????if number == 4 and value == 1:
????????????????i = i + 10
????????????????if i > 90:
????????????????????i = 90
????????????????print(i)
????????????elif number == 0 and value == 1:
????????????????i = i - 10
????????????????if i < 10:
????????????????????i = 10
????????????????print(i)
????????if js.type(type_) == "axis":
????????????print("axis:{} state: {}".format(number, value))
????????????
????????????if number == 7 and value > 32760:
????????????????c.back(i)
????????????elif number == 7 and value < -32760:
????????????????c.run(i)
????????????elif number == 6 and value > 32760:
????????????????c.right(i)
????????????elif number == 6 and value < -32760:
????????????????c.left(i) ?????????
????????????else:
????????????????c.brake()
????????????????
????????????if number == 2:
????????????????# handle_axis(time, value)
????????????????js.x_axis = value * 1.0 / 32767
def main():
????t = threading.Thread(target=joystick_thread, args=())
????t.start()
????#logger.start()
????while not logger.stopped():
????????time.sleep(0.01)
????????logger.log(js.x_axis)
????t.join()
????c.stop()
if __name__ == "__main__":
main()
class Test:
????def Grabbing(self):
????????servo1=Servo(2)
????????servo2=Servo_pwm(2)
????????servo3 = Servo(1)
????
????????servo2.servocontrol(40,100)
????
????????magsens=Magneto_sensor(3)
????????time.sleep(4)
????
????????servo1.servocontrol(-10,90)
????????# servo2.servocontrol(160, 50)
????????time.sleep(2)
????
????????servo2.servocontrol(0,100)
????????print("cccccc")
????????time.sleep(2)
????
????????servo1.servocontrol(-85,60)
????????# servo2.servocontrol(85, 50)
????????time.sleep(2)
grabbing = Test()