Skip to content

Instantly share code, notes, and snippets.

@simonds
Last active March 14, 2017 05:12
Show Gist options
  • Save simonds/31a2bf7ad297bb712f781c2b9d9095ea to your computer and use it in GitHub Desktop.
Save simonds/31a2bf7ad297bb712f781c2b9d9095ea to your computer and use it in GitHub Desktop.
Python script for controlling a Raspberry Pi car.
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")
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment