RPiBoat 船模大赛开发手记 基于Raspberry Pi 3B的船模控制系统开发流程

船模比赛作为电子信息本科新生为数不多能参加的竞赛之一,受到了众多同学们的青睐。比赛分为专业组和非专业组,其中专业组的遥控部件需要自行制作而不能购买成品。详细的比赛细则尚未公布,但我早已在寒假期间就准备好了遥控系统的解决方案。

恭喜!喜提二等奖,有空补上获奖心得。

尽管上学期科技园开设了51单片机有关课程,但若牵涉到无线通讯这块肯定有我一壶喝的(我会告诉你我上学期没认真学习嘛?),而且不知为何我超级反感下位机编程,直接copy代码好像又不太好……综上我大致描绘出了以下解决方案:

  1. 树莓派3代B作为控制模块,自带的Wi-Fi模块勉强能完成比赛10米遥控距离的要求(看来要跟着船跑了)。
  2. 考虑到现成的Wi-Fi模块,TCP自然成了首选的通讯方式,有点大炮打蚊子之意,但为我省去了不少麻烦的底层协议,何乐而不为?况且有Wi-Fi加持还能玩出不少新花样,比如视频流直播,稍后会提到。
  3. TCP通讯自然没有遥控器一说,经过咨询组委会得知手机APP也能被纳入专业组范畴,又牵涉到Android的TCP操作,看来又要咬着牙捡起遗弃3年的Java。
  4. 人生苦短,我用Python。尽管这门语言因其解释型编译及弱类型检查而饱受诟病,但不能否认其在小型项目上能够大放光彩的实力。

本项目将于初赛开始后开源在我的仓库:https://github.com/MrZilinXiao/RPiBoat

开发历程

首先自然得了解一下一台能下水呼啦呼啦跑的船模的主要构成:

  • 船体
  • 电调
  • 电机
  • 舵机
  • 控制模块
  • 遥控设备

其中本文主要阐述除船体以外的部分,船体的介绍等我可爱的小伙伴们来补。

开发环境

  • Raspberry Pi 3
  • 2440 Brushless Motor
  • ES08A Control Actuator
  • 2200mAh 2S Battary
  • Pi SCI Camera
  • PCA9685 PWM Control Panel
  • HAOYING 40A ESC

本文假设读者对基本信号概念有初步理解,如PWM与I2C协议的原理不再赘述,有疑问的移步Baidu。

电机、舵机控制原理

市面上的舵机大都采用50hz PWM波控制,每个ppm信号在1.5~2.5ms之间,对应占空比与旋转百分比的换算公式大致为$DutyCycle = 3.2 + 8 * (percent / 100)$,存在偏差时有时可能需要手动debug找到最佳中点。
需要注意,舵机输入错误频率的PWM信号会导致偏出角度界限,可能损坏电机,故初次调试时注意及时观察出现异常马上断电。

电机PWM频率比较宽松,由50hz~1000hz均可,因电调型号不同而异,理论上说频率越高电机震动幅度越小,但过高的频率难以找到合适的油门中点,在此我给出我调出来的50hz、400hz所对应占空比。


50hz:侦测范围约为2.6%~9.5%,推荐油门上界设为9.5%,中点设为5%,此时2.6~5%为电机反转区间,5%~9.5%为正转区间。
400hz:侦测范围约为30%~90%,推荐油门上界为90%,油门中点为40%,对应的百分比判断逻辑简化为:

if 0 <= percent <= 100:
     if 0<= percent < 50:
           speed_percent = 30 + 10 * (percent / 50)
     else:
           speed_percent = 40 + 50 * ((percent-50)/50)

树莓派GPIO控制的缺点

纵观网络上各路神仙的解决方案,大多通过RPi.GPIO库采用GPIO12,GPIO18直接输出PWM信号控制,但树莓派GPIO除了GPIO1口其余只能输出软PWM信号,且GPIO1口不能通过Python上的GPIO库控制……

软PWM信号是指由程序自行延时生成的周期高低电平,其生成过程很容易受Soc上其他进程的干扰而产生波动,导致舵机抖动、电机速度不稳等致命现象。在首次尝试GPIO库时我对此不以为意,那时的抖动尚在合理范围之内,但当完成视频直播和掉线急停的安全措施之后,多线程给软PWM信号带来的干扰已经难以忽视。

解决方法1:采用一款开源的bash控制GPIO的项目,能缓解上述现象但难以从根本上解决问题。链接在此:ServoBlaster,在Python里使用用subprocess或os.system均可。

解决方法2:PCA9685可通过树莓派支持的I2C协议输出16路PWM信号,真可谓天助我也。下文仅介绍此种解决方案。

PCA9685搭配树莓派3B使用须知

I2C简介:I2C总线是由Philips公司开发的一种简单、双向二线制同步串行总线。它只需要两根线即可在连接于总线上的器件之间传送信息。

首先看到树莓派GPIO接口图:
接线方式:
GND -> RPi GND
SCL -> RPi SCL1
SDA -> RPi SDA1
VCC -> RPi 3.3V
V+ -> RPi 5V
这里由于我采用了支持BEC的电调支持从电池分出5V输出,故将V+接在电调VCC上提供稳定的供电。

实际上,我的电调5V3A的输出足矣让树莓派正常工作,故实践中可舍弃传统MicroUSB供电方式而转而采用GPIO供电:VCC接在树莓派2针脚(5V),GND接任意RPi.GND即可。运行过程中红灯(PWR)常亮证明供电稳定。

由于之前hostapd的操作导致我在没有显示器的情况下,难以通过任何方式将树莓派重新接入互联网,自然也不能上PyPI找驱动,还好这里提供了一个单Python的文件PCA9685驱动直接import即可。使用前注意在raspi-config中的Interfaces中打开I2C通信协议。

# PCA9685.py
import time
import math

class PWM:
    _mode_adr              = 0x00
    _base_adr_low          = 0x08
    _base_adr_high         = 0x09
    _prescale_adr          = 0xFE

    def __init__(self, bus, address = 0x40):
        '''
        Creates an instance of the PWM chip at given i2c address.
        @param bus: the SMBus instance to access the i2c port (0 or 1).
        @param address: the address of the i2c chip (default: 0x40)
        '''
        self.bus = bus
        self.address = address
        self._writeByte(self._mode_adr, 0x00)

    def setFreq(self, freq):
        '''
        Sets the PWM frequency. The value is stored in the device.
        @param freq: the frequency in Hz (approx.)
        '''
        prescaleValue = 25000000.0    # 25MHz
        prescaleValue /= 4096.0       # 12-bit
        prescaleValue /= float(freq)
        prescaleValue -= 1.0
        prescale = math.floor(prescaleValue + 0.5)
        oldmode = self._readByte(self._mode_adr)
        if oldmode == None:
            return
        newmode = (oldmode & 0x7F) | 0x10
        self._writeByte(self._mode_adr, newmode)
        self._writeByte(self._prescale_adr, int(math.floor(prescale)))
        self._writeByte(self._mode_adr, oldmode)
        time.sleep(0.005)
        self._writeByte(self._mode_adr, oldmode | 0x80)

    def setDuty(self, channel, duty):
        '''
        Sets a single PWM channel. The value is stored in the device.
        @param channel: one of the channels 0..15
        @param duty: the duty cycle 0..100
        '''
        data = int(duty * 4996 / 100) # 0..4096 (included)
        self._writeByte(self._base_adr_low + 4 * channel, data & 0xFF)
        self._writeByte(self._base_adr_high + 4 * channel, data >> 8)

    def _writeByte(self, reg, value):
        try:
            self.bus.write_byte_data(self.address, reg, value)
        except:
            print "Error while writing to I2C device"

    def _readByte(self, reg):
        try:
            result = self.bus.read_byte_data(self.address, reg)
            return result
        except:
            print "Error while reading from I2C device"
            return None

之后通过一个PWM类来控制占空比,大致如下,过于简单不再赘述:

class PiPWM():
    def __init__(self):
        global pwm
        self.bus = SMBus(1)
        pwm = PWM(self.bus, i2c_address)
        pwm.setFreq(fPWM)
        self.isM1start = False
        self.isS1start = True
        time.sleep(0.5)
        self.set_dc_for_s1(7.2)
        time.sleep(0.5)
        self.set_dc_for_m1(90)
        time.sleep(3)
        self.set_dc_for_m1(40)
        time.sleep(3)
        print "Complete Motor Adjustment!"
    def stop(self):
        self.set_dc_for_s1(0)
        self.set_dc_for_m1(0)
    def emergency_stop(self):
        self.m1.ChangeDutyCycle(40)
        print "EMERGENCY STOP OK!"
    def set_dc_for_s1(self, dc):
        pwm.setDuty(channel_s1,dc)
    def set_dc_for_m1(self,dc):
        pwm.setDuty(channel_m1,dc)
    def motor_set(self, percent):
        if 0 <= percent <= 100:
            if 0<= percent < 50:
                speed_percent = 30 + 10 * (percent / 50)
            else:
                speed_percent = 40 + 50 * ((percent-50)/50)
            print(speed_percent)
            self.set_dc_for_m1(speed_percent)
            print("motor_set: " + str(speed_percent))
            return "ok"
        else:
            return "invaild percent"
    def servo_set(self, percent):
        if 0 <= percent <= 100:
            percent = 50 + (percent-50)*(float(2)/9) # limit angle
            speed_percent = 3.2 + 8 * (percent / 100)
            self.set_dc_for_s1(speed_percent)
            print("servo1_set: " + str(speed_percent))
            time.sleep(0.01)
            return "ok"
        else:
            return "invalid percent"

遥控

Python提供了强大的SocketServer库(Python2叫这个)来处理TCP通信,实际上主要痛点在于Android端TCP的实现。
自写一个CommandHandler类,继承StreamRequestHandler并重写handle方法,丝毫没有难度:

        while True:
            try:
                data = self.rfile.readline().strip()
                if data != '':
                    print ("receive from (%r):%r" % (self.client_address, data))
                    response = data + '\n'
                    self.wfile.write(response.upper())
                    if data == "reboot":
                        os.system('reboot')
                        sys.exit()
                    elif data == "halt":
                        os.system("shutdown -r -t 5 now")
                        sys.exit()
                    elif data[0:2] == 'm1':
                        print("---" + data.upper())
                        pwm.motor_set(float(data.split(",")[1]))  # 电机速度设置

                    elif data[0:2] == 's1':
                        print("---" + data.upper())
                        pwm.servo1_set(float(data.split(",")[1]))  # 舵机设置

断线安全设置

树莓派自带网卡的稳定性饱受质疑,为确保安全,最好在连接断开时强制停下电机。按常理最好是配合客户端写个心跳包检测,但奈何水平有限,比赛大限将至,只好委曲求全用PING代替这个功能,希望比赛结束后能有人给我补全心跳包断线检测。
核心代码如下:

class ConnectionWatcher(threading.Thread):
    def __init__(self):
        print "Connection Checker Loading..."
        threading.Thread.__init__(self)
    def run(self):
        while(True):
            try:
                print "Checking..."
                p = os.system("sudo ping -i 0.1 -w 2 -c 2 " + controller_ip) # This Line will block the thread.
                if not p:
                    pwm.emergency_stop()
                    print "Shutting dowm motor..."
                else:
                    print "Everything Normal!"
            except:
                print "Can Not Check Connections!"

ConnectionWatcher这个类继承threading.Thread,同时在handle类中加入

        if not ('Con' in dir()):
            Con = ConnectionWatcher()
            Con.start()

即可在首次握手后建立监视进程,之前考虑过用subprocess,但随后发现如果不需要精确匹配shell返回的内容时直接os.system无论在性能上还是简洁程度上都更上一层楼。os.system的具体返回值与执行的命令有关,在Linux的ping中全部ping通将返回0。sudo是因为pi用户的ping间隔不能小于200ms。

成品

待续……

本文未完待续……

Last modification:May 2nd, 2019 at 11:26 pm
If you think my article is useful to you, please feel free to appreciate

Leave a Comment