第1步：準(zhǔn)備

步驟2：代碼結(jié)構(gòu)

我將代碼劃分為這些文件，我將一一解釋：

aum2.py

l298NDrive.py

remoteControl.py

utils.py

在接下來的步驟中，我將一步一步地解釋這些文件

第3步：Aum2.py

這是應(yīng)用程序的主要入口點，包含機器人類

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（）

有些我要對此文件發(fā)表評論的要點：

我正在使用扭曲庫來協(xié)調(diào)來自UDP的遠(yuǎn)程控制訂單服務(wù)器（將在相應(yīng)的源文件中顯示）和機器人的主要Run方法。如您所見，該方法現(xiàn)在什么也不做，但是其想法是在其中添加代碼以使機器人更具自治性。

processRemoteCommand 方法將是

開始方法是在我配置需要協(xié)調(diào)的兩件事時：監(jiān)聽

第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應(yīng)用的Android手機發(fā)送的UDP程序包。此應(yīng)用程序發(fā)送具有以下結(jié)構(gòu)的消息：

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 將此消息的有用參數(shù)轉(zhuǎn)換為機器人所需的x和y值，其中

x 是從前到后的軸，而

y 則是從一側(cè)到另一側(cè)。

我也有z軸的代碼，但是我暫時不使用它。

步驟5：L298NDrive.py

這是負(fù)責(zé)控制電動機的文件。為此，它將命令發(fā)送到16伺服驅(qū)動器，該驅(qū)動器連接到L298N電動機驅(qū)動器。我想我可能會在我構(gòu)建的每個Raspberry Pi機器人上重用該類。它確實很簡單，并且不需要復(fù)雜的處理即可完成它的工作。

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 =左），并告訴伺服驅(qū)動器如何處理與該電動機關(guān)聯(lián)的引腳。

也許應(yīng)該給出一些解釋以了解我為什么要執(zhí)行我的工作

L298N電機驅(qū)動器

使用6個引腳驅(qū)動兩個電機：

enA 用來告訴速度（使用PWM），我們要使電動機啟動1。通電后，告訴駕駛員電動機1應(yīng)該沿一個方向移動（in2應(yīng)該處于關(guān)閉狀態(tài)）

in2 通電時，告訴駕駛員電動機1應(yīng)當(dāng)沿相反方向移動（in1應(yīng)該處于關(guān)閉狀態(tài)）

in3 通電時，告知駕駛員電機2應(yīng)該沿一個方向移動（in4應(yīng)該關(guān)閉）

in4 告訴駕駛員，電動機2應(yīng)該沿相反方向移動（in4應(yīng)該關(guān)閉）

enB 用于告訴速度（使用PWM），我們希望電動機轉(zhuǎn)動

所以在我們的例子中，因為我對所有引腳都使用了伺服驅(qū)動器（只有其中兩個需要PWM）：

當(dāng)我做到這一點：

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

我真的是在告訴伺服驅(qū)動器，在所有PWM期間，該引腳（in1）應(yīng)該為OFF，（所以我將該引腳設(shè)為OFF）

當(dāng)我這樣做時：

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

我實際上是在告訴伺服驅(qū)動器，在所有PWM周期中，該引腳（in1）應(yīng)該處于ON位置（所以我將其打開

然后，當(dāng)我這樣做時：

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

我使用真正的PWM引腳，并使用速度參數(shù)設(shè)置我所要求的脈沖。

步驟6：Utils.py

最后，這是最后的平安。它不包含類，但是我認(rèn)為我會在其他地方重用的所有有用功能。

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）內(nèi)的值（v）轉(zhuǎn)換為輸出范圍（out_min，out_max）內(nèi)的相應(yīng)值。

我為輸入值添加了限制，所以我只接受av，位于in_min和in_max之內(nèi)。

我擁有的另一個函數(shù)（ joystickToDiff ）稍微復(fù)雜一點，但是其目的是從輸入的操縱桿對轉(zhuǎn)換值，以達到差動驅(qū)動機器人左右電機所需的速度。此輸出已交付，已轉(zhuǎn)換為指定為參數(shù)的速度范圍。

您可以在我的博客中找到有關(guān)此部分的更多詳細(xì)信息

步驟7：更多圖片

這里還有Aum2機器人的更多圖片