python 用于控制Raspberry Pi汽车的Python脚本。

Posted 2021-05-09

import time
time.sleep(10)

from rrb3 import *
from bluetooth import *

speedFactor = 0.1

class Robot(object):

    def __init__(self, leftMotorSpeed, rightMotorSpeed, direction):
        self.leftMotorSpeed = leftMotorSpeed
        self.rightMotorSpeed = rightMotorSpeed
        self.direction = direction

    # rr.set_motors(left speed: 0-1, left motor direction: 0=forward, 1=backward , right speed: 0-1, right motor direction)
    
    def doMotors(self):
        rr.set_motors(self.leftMotorSpeed , self.direction, self.rightMotorSpeed, self.direction)

    def increaseSpeed(self):
        if self.leftMotorSpeed > self.rightMotorSpeed:
            self.leftMotorSpeed = self.rightMotorSpeed
        elif self.rightMotorSpeed > self.leftMotorSpeed:
            self.rightMotorSpeed = self.leftMotorSpeed
        else:
            if self.leftMotorSpeed < 1:
                self.leftMotorSpeed += speedFactor
            if self.rightMotorSpeed < 1:
                self.rightMotorSpeed += speedFactor
        self.doMotors()

    def decreaseSpeed(self):
        if self.leftMotorSpeed > self.rightMotorSpeed:
            self.leftMotorSpeed = self.rightMotorSpeed
        elif self.rightMotorSpeed > self.leftMotorSpeed:
            self.rightMotorSpeed = self.leftMotorSpeed
        else:
            if self.leftMotorSpeed > 0:
                self.leftMotorSpeed -= speedFactor
            if self.rightMotorSpeed > 0:
                self.rightMotorSpeed -= speedFactor
        self.doMotors()

    def turnLeft(self):        
        if self.leftMotorSpeed > 0:
            self.leftMotorSpeed -= speedFactor
        if self.rightMotorSpeed < 1:
            self.rightMotorSpeed += speedFactor
        self.doMotors()

    def turnRight(self):
        if self.leftMotorSpeed < 1:
            self.leftMotorSpeed += speedFactor
        if self.rightMotorSpeed > 0:
            self.rightMotorSpeed -= speedFactor
        self.doMotors()

    def stop(self):
        self.leftMotorSpeed = 0
        self.rightMotorSpeed = 0
        self.doMotors()


def blinkLed(ledNumber):
    delay = 0.2
    if ledNumber == 1:
        rr.set_led1(1)
        time.sleep(delay)
        rr.set_led1(0)
        time.sleep(delay)
    if ledNumber == 2:
        rr.set_led2(1)
        time.sleep(delay)
        rr.set_led2(0)
        time.sleep(delay)

def signalReady():
    rr.set_led1(1)
    rr.set_led2(1)

def signalConnected():
    rr.set_led1(0)
    rr.set_led2(0)
    blinkLed(1)
    blinkLed(2)
    blinkLed(1)
    blinkLed(2)
    blinkLed(1)
    blinkLed(2)



# Configure the RRB
BATTERY_VOLTS = 6
MOTOR_VOLTS = 6
rr = RRB3(BATTERY_VOLTS, MOTOR_VOLTS)

server_sock=BluetoothSocket( RFCOMM )
server_sock.bind(("",PORT_ANY))
server_sock.listen(1)

port = server_sock.getsockname()[1]

uuid = "94f39d29-7d6d-437d-973b-fba39e49d4ee"

advertise_service( server_sock, "SampleServer",
                   service_id = uuid,
                   service_classes = [ uuid, SERIAL_PORT_CLASS ],
                   profiles = [ SERIAL_PORT_PROFILE ]
                    )
                   
print("Waiting for connection on RFCOMM channel %d" % port)

signalReady()

client_sock, client_info = server_sock.accept()
print("Accepted connection from ", client_info)

signalConnected()

exitLoop = 0
car = Robot(0, 0, 0)

try:
    while exitLoop == 0:
        data = client_sock.recv(1024)
        if len(data) == 0: break
        print("received [%s]" % data)

        if data == 'f':
            car.increaseSpeed()
        elif data == 'l':
            car.turnLeft()
        elif data == 's':
            car.stop()
        elif data == 'r':
            car.turnRight()
        elif data == 'b':
            car.decreaseSpeed()
        elif data == 'x':
            exitLoop = 1
except IOError:
    pass
except KeyboardInterrupt:
    pass

print("disconnected")

client_sock.close()
server_sock.close()
GPIO.cleanup()

print("all done")