Earlier this week I was asked to put together a robotics demo for the Electrical and Computer Engineering first year discipline night. This is the night where we try and entice the first years (Queen’s is a general first year) to choose ECE for their discipline for second and hopefully subsequent years. Now I could have ran any number of basic demonstrations that would have taken no time at all but of course I chose to take the opportunity to do something cool (well at least I think it is cool…).
Being the teaching assistant for ELEC 299, a second year course which uses mobile robots, I had access to some pretty cool hardware. I’d been playing with the robots for a while so I knew all the basic features now it was time to go above and beyond.
I hooked up the bluetooth shield and started looking into how to send commands from my computer. I had a program from previous years in the course that did just this but it was in the form of a windows binary and I a) couldn’t be bothered to boot into windows and b) wanted to do it myself anyways. I googled around for a bluetooth library and decided on pybluez with python. It took me a bit to get set up to send data between the two but it wasn’t too tough. I borrowed a getch() class from stack overflow to facilitate only grabbing one key press at a time and then sent it to the robot. Of course I chose the familiar control scheme of W S A D`.
Here is the python code, it’s pretty simple:
from bluetooth import *
from getch import getch
MAC_ADR = "00:3C:B8:B1:14:22"
# Discovery
#print "performing inquiry..."
#nearby_devices = discover_devices(lookup_names = True)
#print "found %d devices" % len(nearby_devices)
#for name, addr in nearby_devices:
#print " %s - %s" % (addr, name)
# Create the client socket
client_socket = BluetoothSocket( RFCOMM )
client_socket.connect((MAC_ADR, 1))
print "Connected"
print "Press 'q' to quit"
key = 0;
while key != 'q':
key = getch() #gets 1 key only
print key
client_socket.send(key)
# Close the connection
client_socket.close()
and the getch class
class _Getch:
"""Gets a single character from standard input. Does not echo to the screen."""
def __init__(self):
try:
self.impl = _GetchWindows()
except ImportError:
self.impl = _GetchUnix()
def __call__(self): return self.impl()
class _GetchUnix:
def __init__(self):
import tty, sys
def __call__(self):
import sys, tty, termios
fd = sys.stdin.fileno()
old_settings = termios.tcgetattr(fd)
try:
tty.setraw(sys.stdin.fileno())
ch = sys.stdin.read(1)
finally:
termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
return ch
class _GetchWindows:
def __init__(self):
import msvcrt
def __call__(self):
import msvcrt
return msvcrt.getch()
getch = _Getch()
After I got the connection stuff working and out of the way it was pretty simple to write an Arduino program to accept my input and respond accordingly.
But I wasn’t done just yet! I wanted to take my demo further and use an XBox 360 controller instead of the keyboard. It turns out this really wasn’t too hard I used pygame to read from the joystick and the rest is pretty much history. Now I will admit at this point I had pretty much proved my point and I needed to get back to more important work so the final result was a bit of a cop out. Rather than modify my Arduino program to read analog data over serial and really use the controller I simply mapped joystick values to W A S D in python before sending it over bluetooth. It would be really cool to come back and finish this properly but for now my demo was done.
XBox 360 Controller python code:
#!/usr/bin/env python
import bluetooth
import serial
import pygame
import time
import math
# init controller
pygame.init()
controller = pygame.joystick.Joystick(0)
controller.init()
print 'Xbox Controller Connected'
# Create the client socket
MAC_ADR = "00:3C:B8:B1:14:22"
client_socket = bluetooth.BluetoothSocket( bluetooth.RFCOMM )
client_socket.connect((MAC_ADR, 1))
print "Bluetooth Connected"
print ' '
print ' '
print '/**************************/'
print 'Joystick Drive Program'
print "Press 'q' to quit"
print '/**************************/'
key = 0
y = 0
x = 0
while key != 'q':
for event in pygame.event.get():
if event.type == pygame.JOYAXISMOTION:
if event.axis == 1:
y = event.value
if math.fabs(y) < 0.2:
y = 0
if event.axis == 3: #4 in windows, 3 in linux
x = event.value
if math.fabs(x) < 0.2:
x = 0
# send to arduino
command = ' '
if y < 0:
command = 'w'
elif y > 0:
command = 's'
elif x < 0:
command = 'a'
elif x > 0:
command = 'd'
print command
client_socket.send(command)
print client_socket.recv(1024)
# Close the connection
client_socket.close()
And finally the Arduino Program:
// bluetoothDrive
// Kevin Hughes
// 2012
// Motor Pins
int E1 = 6;
int M1 = 7;
int E2 = 5;
int M2 = 4;
void setup()
{
// set pin modes
pinMode(E1, OUTPUT);
pinMode(M1, OUTPUT);
pinMode(E2, OUTPUT);
pinMode(M2, OUTPUT);
// init
Serial.begin(115200);
}
void loop()
{
int command;
if(Serial.available()) {
command = Serial.read();
// Moving
if(command==119)
driveForwards();
if(command==115)
driveReverse();
if(command==97)
turnLeft();
if(command==100)
turnRight();
if(command==32)
driveStop();
}// end if
}
// Subroutines and Functions
void driveForwards() {
digitalWrite(M1,HIGH);
digitalWrite(M2,HIGH);
analogWrite(E1,100);
analogWrite(E2,100);
}
void driveReverse() {
digitalWrite(M1,LOW);
digitalWrite(M2,LOW);
analogWrite(E1,100);
analogWrite(E2,100);
}
void driveStop() {
digitalWrite(M1,HIGH);
digitalWrite(M2,HIGH);
analogWrite(E1,0);
analogWrite(E2,0);
}
void turnLeft() {
digitalWrite(M1,HIGH);
digitalWrite(M2,LOW);
analogWrite(E1,100);
analogWrite(E2,100);
}
void turnRight() {
digitalWrite(M1,LOW);
digitalWrite(M2,HIGH);
analogWrite(E1,100);
analogWrite(E2,100);
}
Pretty simple really it just waits for a serial command, checks if it matches W A S or D and then executes the appropriate code.
Hope you enjoyed this, the demo was a hit at the discipline night!
