Last active
March 14, 2017 05:12
-
-
Save simonds/31a2bf7ad297bb712f781c2b9d9095ea to your computer and use it in GitHub Desktop.
Python script for controlling a Raspberry Pi car.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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