基于RaspberryPi Zero W的机器人的制作

第1步：准备

步骤2：代码结构

我将代码划分为这些文件，我将一一解释：

aum2.py

l298NDrive.py

remoteControl.py

utils.py

在接下来的步骤中，我将一步一步地解释这些文件

第3步：Aum2.py

这是应用程序的主要入口点，包含机器人类

import RPi.GPIO as GPIO

from twisted.internet import reactor， task

import l298NDrive

import remoteControl

import logging

import utils

logging.basicConfig（level=logging.WARN）

log = logging.getLogger（‘Aum2’）

enableMotors = True # Motors can be disabled for testing with this

runFrequency = 1 # The frequency on wich the main Task will be ran

class Aum2（object）：

def __init__（self）：

log.debug（“Initializing Aum2 robot.。.”）

# Configuration

self.port = 5555 # Port to open so the remote can connect

GPIO.setmode（GPIO.BOARD）

# Configure min and max servo pulse lengths

# Keep in mind these are not pins but servo outs

self.motors = l298NDrive.L298NDrive（10， 11， 12， 13， 14， 15， enableMotors）

self.rc = remoteControl.RemoteControl（self.port）

def processRemoteCommand（self， cmd）：

（vr， vl） = utils.joystickToDiff（cmd.x， cmd.y， -9.8， 9.8， -4095， 4095）

self.motors.setSpeeds（int（vr）， int（-vl））

def run（self）：

print（“running.。.”）

def start（self）：

self.rc.start（self.processRemoteCommand）

l = task.LoopingCall（self.run）

l.start（runFrequency） # run will be called with this frequency

# Gather our code in a main（） function

def main（）：

robot = Aum2（）

robot.start（）

reactor.run（）

# Standard boilerplate to call the main（） function to begin

# the program.

if __name__ == ‘__main__’：

main（）

有些我要对此文件发表评论的要点：

我正在使用扭曲库来协调来自UDP的远程控制订单服务器（将在相应的源文件中显示）和机器人的主要Run方法。如您所见，该方法现在什么也不做，但是其想法是在其中添加代码以使机器人更具自治性。

processRemoteCommand 方法将是

开始方法是在我配置需要协调的两件事时：监听

第4步：RemoteControl.py

此文件包含两个类，即RemoteControl类， RCCommand类

import logging

import traceback

from twisted.internet.protocol import DatagramProtocol

from twisted.internet import reactor

import utils

log = logging.getLogger（‘RemoteControl’）

class RemoteControl（DatagramProtocol）：

def __init__（self， port）：

self.port = port

def start（self， onCommandReceived）：

log.debug（“Initializing RemoteControl.。.”）

self.onCommandReceived = onCommandReceived

reactor.listenUDP（self.port， self）

def datagramReceived（self， data， addr）：

try：

log.debug（“Received %r from %s” % （data， addr））

command = RCCommand（data）

self.onCommandReceived（command）

except （KeyboardInterrupt， SystemExit）：

raise

except：

traceback.print_exc（）

class RCCommand（object）：

# Timestamp ［sec］， sensorid， x， y， z， sensorid， x， y， z， sensorid， x， y， z

# 890.71558， 3， 0.076， 9.809， 0.565， 。..

def __init__（self， message）：

self.x = 0

self.y = 0

self.z = 0

self.values = message.split（“，”）

if len（self.values） 》= 5 and self.values［1］.strip（） == “3”：

self.x = -float（self.values［3］.strip（））

self.y = -float（self.values［2］.strip（））

self.z = float（self.values［4］.strip（））

log.debug（“x={} y={} z={}”.format（self.x， self.y， self.z））

else：

log.warn（“Invalid message： {}”.format（message））

同样是RemoteControl类，它使用扭曲的框架来侦听运行Wireless IMU应用的Android手机发送的UDP程序包。此应用程序发送具有以下结构的消息：

890.71558， 3、0.076、9.809、0.565 ，4，-0.559、0.032，-0.134、5 -21.660，-36.960， -28.140

我只对上面突出显示的消息部分感兴趣，这就是传感器ID 3 ，以及加速度计提供的三个测量值（ 0.076、9.809、0.565 ）。

您可以在代码中看到，类 RCCommand 将此消息的有用参数转换为机器人所需的x和y值，其中

x 是从前到后的轴，而

y 则是从一侧到另一侧。

我也有z轴的代码，但是我暂时不使用它。

步骤5：L298NDrive.py

这是负责控制电动机的文件。为此，它将命令发送到16伺服驱动器，该驱动器连接到L298N电动机驱动器。我想我可能会在我构建的每个Raspberry Pi机器人上重用该类。它确实很简单，并且不需要复杂的处理即可完成它的工作。

import Adafruit_PCA9685

import logging

import math

import utils

log = logging.getLogger（‘L298NDrive’）

class L298NDrive（object）：

def __init__（self， enA， in1， in2， in3， in4， enB， enabled）：

log.debug（“Initializing L298Drive.。.”）

self.minPwm = 0

self.MaxPwm = 4095

self.enA = enA

self.in1 = in1

self.in2 = in2

self.in3 = in3

self.in4 = in4

self.enB = enB

self.enabled = enabled

if enabled：

self.enable（）

def enable（self）：

self.enabled = True

# Initialise the PCA9685 using the default address （0x40）。

self.pwm = Adafruit_PCA9685.PCA9685（）

self.setSpeeds（0， 0）

def disable（self）：

self.enabled = False

self.setSpeeds（0， 0）

# Initialise the PCA9685 using the default address （0x40）。

self.pwm = None

def setSpeeds（self， rSpeed， lSpeed）：

self.setSpeed（1， rSpeed）

self.setSpeed（2， lSpeed）

def setSpeed（self， motor， speed）：

pwm = int（math.fabs（speed））

log.info（“Motor： {} speed： {} pwm： {}”.format（motor， speed， pwm））

if motor == 1：

if speed 》= 0：

if self.enabled：

self.pwm.set_pwm（self.in1， 0， 0）

self.pwm.set_pwm（self.in2， 0， self.MaxPwm）

else：

if self.enabled：

self.pwm.set_pwm（self.in1， 0， self.MaxPwm）

self.pwm.set_pwm（self.in2， 0， 0）

if self.enabled：

self.pwm.set_pwm（self.enA， 0， pwm）

else： # motor == 2

if speed 》= 0：

if self.enabled：

self.pwm.set_pwm（self.in3， 0， 0）

self.pwm.set_pwm（self.in4， 0， self.MaxPwm）

else：

if self.enabled：

self.pwm.set_pwm（self.in3， 0， self.MaxPwm）

self.pwm.set_pwm（self.in4， 0， 0）

if self.enabled：

self.pwm.set_pwm（self.enB， 0， pwm）

可以看出，代码的主要部分在setSpeed（self，motor，speed）方法中，该方法接收要配置的电动机编号（1 =右，2 =左），并告诉伺服驱动器如何处理与该电动机关联的引脚。

也许应该给出一些解释以了解我为什么要执行我的工作

L298N电机驱动器

使用6个引脚驱动两个电机：

enA 用来告诉速度（使用PWM），我们要使电动机启动1。通电后，告诉驾驶员电动机1应该沿一个方向移动（in2应该处于关闭状态）

in2 通电时，告诉驾驶员电动机1应当沿相反方向移动（in1应该处于关闭状态）

in3 通电时，告知驾驶员电机2应该沿一个方向移动（in4应该关闭）

in4 告诉驾驶员，电动机2应该沿相反方向移动（in4应该关闭）

enB 用于告诉速度（使用PWM），我们希望电动机转动

所以在我们的例子中，因为我对所有引脚都使用了伺服驱动器（只有其中两个需要PWM）：

当我做到这一点：

self.pwm.set_pwm（self.in1， 0， 0）

我真的是在告诉伺服驱动器，在所有PWM期间，该引脚（in1）应该为OFF，（所以我将该引脚设为OFF）

当我这样做时：

self.pwm.set_pwm（self.in1， 0， self.MaxPwm）

我实际上是在告诉伺服驱动器，在所有PWM周期中，该引脚（in1）应该处于ON位置（所以我将其打开

然后，当我这样做时：

self.pwm.set_pwm（self.enA， 0， pwm）

我使用真正的PWM引脚，并使用速度参数设置我所要求的脉冲。

步骤6：Utils.py

最后，这是最后的平安。它不包含类，但是我认为我会在其他地方重用的所有有用功能。

import math

import logging

log = logging.getLogger（‘utils’）

def map（v， in_min， in_max， out_min， out_max）：

# Check that the value is at least in_min

if v 《 in_min：

v = in_min

# Check that the value is at most in_max

if v 》 in_max：

v = in_max

return （v - in_min） * （out_max - out_min） // （in_max - in_min） + out_min

def joystickToDiff（x， y， minJoystick， maxJoystick， minSpeed， maxSpeed）：

# If x and y are 0， then there is not much to calculate.。.

if x == 0 and y == 0：

return （0， 0）

# First Compute the angle in deg

# First hypotenuse

z = math.sqrt（x * x + y * y）

# angle in radians

rad = math.acos（math.fabs（x） / z）

# and in degrees

angle = rad * 180 / math.pi

# Now angle indicates the measure of turn

# Along a straight line， with an angle o， the turn co-efficient is same

# this applies for angles between 0-90， with angle 0 the coeff is -1

# with angle 45， the co-efficient is 0 and with angle 90， it is 1

tcoeff = -1 + （angle / 90） * 2

turn = tcoeff * math.fabs（math.fabs（y） - math.fabs（x））

turn = round（turn * 100， 0） / 100

# And max of y or x is the movement

mov = max（math.fabs（y）， math.fabs（x））

# First and third quadrant

if （x 》= 0 and y 》= 0） or （x 《 0 and y 《 0）：

rawLeft = mov

rawRight = turn

else：

rawRight = mov

rawLeft = turn

# Reverse polarity

if y 《 0：

rawLeft = 0 - rawLeft

rawRight = 0 - rawRight

# minJoystick， maxJoystick， minSpeed， maxSpeed

# Map the values onto the defined rang

rightOut = map（rawRight， minJoystick， maxJoystick， minSpeed， maxSpeed）

leftOut = map（rawLeft， minJoystick， maxJoystick， minSpeed， maxSpeed）

log.debug（“x={} y={}， rOut={}， lOut={}”.format（x， y， rightOut， leftOut））

return （rightOut， leftOut）

这里只有两种方法，第一种是从地图功能的Arduino。它将指定输入限制（in_min，in_max）内的值（v）转换为输出范围（out_min，out_max）内的相应值。

我为输入值添加了限制，所以我只接受av，位于in_min和in_max之内。

我拥有的另一个函数（ joystickToDiff ）稍微复杂一点，但是其目的是从输入的操纵杆对转换值，以达到差动驱动机器人左右电机所需的速度。此输出已交付，已转换为指定为参数的速度范围。

您可以在我的博客中找到有关此部分的更多详细信息

步骤7：更多图片

这里还有Aum2机器人的更多图片