Question

我正在制作一个机器人项目，其中有4个伺服器（2个驱动和2个转向）和2个独立的无刷电机。我的意图是在2个BLDC以指定速度旋转时能够驱动和操纵机器人。另外，我正在使用XboxController库，以便使用控制器来驱动和操纵。我想到了多线程，这就是我想到的代码。但是，一旦第二个线程（init2Thread）启动，它将永远循环，并且我无法操纵机器人，也无法运行BLDC。是否有任何方法可以在后台运行BLDC并操纵/驱动机器人？ >

import math
import os
import pigpio
import sys
import time
import XboxController2
from threading import Thread

pi = pigpio.pi()

#servo pin values setup
ESCf = 17
ESCr = 22
RDservoPin = 23
LDservoPin = 4
RSservoPin = 19
LSservoPin = 21

pi.set_servo_pulsewidth(ESCf, 0)
pi.set_servo_pulsewidth(ESCr, 0)

class gamepadControl:

    def __init__(self):

        #setup controller values
    self.xValue = 0
    self.yValue = 0
    self.xrValue = 0
    self.yrValue = 0

    #create xbox controller class
    self.xboxCont = XboxController2.XboxController(deadzone = 30, scale = 100, invertYAxis = False)

    #setup call backs
    self.xboxCont.setupControlCallback(self.xboxCont.XboxControls.LTHUMBX, self.leftThumbX)
    self.xboxCont.setupControlCallback(self.xboxCont.XboxControls.LTHUMBY, self.leftThumbY)
    self.xboxCont.setupControlCallback(self.xboxCont.XboxControls.RTHUMBY, self.rightThumbX)
    self.xboxCont.setupControlCallback(self.xboxCont.XboxControls.RTHUMBY, self.rightThumbY)
    self.xboxCont.setupControlCallback(self.xboxCont.XboxControls.XBOX, self.xboxButton)
    self.xboxCont.setupControlCallback(self.xboxCont.XboxControls.START, self.startButton)
    self.xboxCont.setupControlCallback(self.xboxCont.XboxControls.A, self.aButton)
    self.xboxCont.setupControlCallback(self.xboxCont.XboxControls.B, self.bButton)

    #start the controller
    self.xboxCont.start()
    self.running = True

#callback funtions for left thumb stick
def leftThumbX(self, xValue):
    self.xValue = xValue
    self.updateMotors()

def leftThumbY(self, yValue):
    self.yValue = yValue
    self.updateMotors()

#callback funtions for right thumb stick
def rightThumbX(self, xrValue):
    self.xrValue = xrValue
    self.updateMotors()

def rightThumbY(self, yrValue):
    self.yrValue = yrValue
    self.updateMotors()

#callback funcion for xbox button
def xboxButton(self, value):
    global RDservoPin
    global LDservoPin
    global ESCf
    global ESCr

    pi.set_servo_pulsewidth(RDservoPin, 0)
    pi.set_servo_pulsewidth(LDservoPin, 0)

    pi.set_servo_pulsewidth(ESCf, 0)
    pi.set_servo_pulsewidth(ESCr, 0)

    self.stop()

def startButton(value):
    if value == 1:
        print value

#callback function for X button
def aButton(self, value):
    if value == 1:
        print value

#callback function for O button
def bButton(self, value):
    if value == 1:
        print value
#stop
def stop(self):
    self.xboxCont.stop()
    self.running = False
    pi.stop() 

def updateMotors(self):
    #angle calculations
    angle = math.degrees(math.atan2(self.xValue, self.yValue))
    length = math.hypot(self.xValue, self.yValue)

    #servo motors pin setup
    global RDservoPin
    global LDservoPin
    global RSservoPin
    global LSservoPin

    #setting pins to output
    pi.set_mode(RDservoPin, pigpio.OUTPUT)
    pi.set_mode(LDservoPin, pigpio.OUTPUT)
    pi.set_mode(RSservoPin, pigpio.OUTPUT)
    pi.set_mode(LSservoPin, pigpio.OUTPUT)

    #setting motors starting values
    pi.set_servo_pulsewidth(RDservoPin, 0)
    pi.set_servo_pulsewidth(LDservoPin, 0)
    pi.set_servo_pulsewidth(RSservoPin, 1500)
    pi.set_servo_pulsewidth(LSservoPin, 1500)

    #conditions
    if angle == 180 and length >= 50:
        pi.set_servo_pulsewidth(RDservoPin, 600)
        pi.set_servo_pulsewidth(LDservoPin, 1600)

        if self.yrValue < 0:
            pw = ((self.yrValue * 2000) / 284) + 1500
            pi.set_servo_pulsewidth(RSservoPin, pw)
            pi.set_servo_pulsewidth(LSservoPin, pw)

        elif self.yrValue > 0:
            pw = ((self.yrValue * 2000) / 284) + 1500
            pi.set_servo_pulsewidth(RSservoPin, pw)
            pi.set_servo_pulsewidth(LSservoPin, pw)

    elif angle == 0 and length >= 50:
        pi.set_servo_pulsewidth(RDservoPin, 1600)
        pi.set_servo_pulsewidth(LDservoPin, 600)

        if self.yrValue < 0:
            pw = ((self.yrValue * 2000) / 284) + 1500
            pi.set_servo_pulsewidth(RSservoPin, pw)
            pi.set_servo_pulsewidth(LSservoPin, pw)

        elif self.yrValue > 0:
            pw = ((self.yrValue * 2000) / 284) + 1500
            pi.set_servo_pulsewidth(RSservoPin, pw)
            pi.set_servo_pulsewidth(LSservoPin, pw)

    elif self.yrValue < 0:
        pw = ((self.yrValue * 2000) / 284) + 1500
        pi.set_servo_pulsewidth(RSservoPin, pw)
        pi.set_servo_pulsewidth(LSservoPin, pw)

    elif self.yrValue > 0:
        pw = ((self.yrValue * 2000) / 284) + 1500
        pi.set_servo_pulsewidth(RSservoPin, pw)
        pi.set_servo_pulsewidth(LSservoPin, pw)

    elif angle == 0 and length == 0 and self.yrValue == 0: 
        pi.set_servo_pulsewidth(RDservoPin, 0)
        pi.set_servo_pulsewidth(LDservoPin, 0)

        pi.set_servo_pulsewidth(RSservoPin, 1500)
        pi.set_servo_pulsewidth(LSservoPin, 1500)

class runEngines:

def __init__(self):

    #start
    self.running = True

    global ESCf
    global ESCr

    pi.set_servo_pulsewidth(ESCf, 1000)
    pi.set_servo_pulsewidth(ESCr, 1000)
    time.sleep(1)

def run(self):
    global ESCf
    global ESCr

    while True:
        pi.set_servo_pulsewidth(ESCf, 1030)
        pi.set_servo_pulsewidth(ESCr, 1030)

if __name__ == '__main__':
print ("START")
try:
    #create class
    init = gamepadControl()

    updateMotors = init.updateMotors()
    initThread = Thread(target = updateMotors)
    initThread.start()

    init2 = runEngines()
    run = init2.run()
    init2Thread = Thread(target = run)
    init2Thread.start()

#Ctrl C
except KeyboardInterrupt:
    print ("USER CANCELLED")
#Error
except:
    print ("ERROR:"), sys.exc_info()[0]
    raise

finally:
    print 
    print ("STOP")
    #if its still running (probably because an error occured, stop it
    if init.running == True:
        init.stop()

如何在python中实现多线程-机器人项目

0 个答案: