class Controller(object):
"""Control E-Puck robot.
Give commands to e-puck robot via bluetooth connection. Uses modified BTcom
tool.
Atributes:
MAX_SPEED -- Maximum speed of e-puck's motors measured in pulses per
second. -MAX_SPEED is maximum backward speed.
"""
MAX_SPEED = 1000
GREYSCALE_MODE = 0
RGB565_MODE = 1
def __init__(self, port, asynchronous=False, timeout=0.5, max_tries=10):
"""Create new controller.
Arguments:
port -- The device where e-puck robot is connected (e.g. /dev/rfcomm0).
asynchronous -- Set True to use asynchronous communication.
Synchronous communication is default.
timeout -- How long to wait before the message is sent again.
max_tries -- How many tries before raising an exception (in async).
"""
try:
if asynchronous:
self.comm = AsyncComm(port, timeout, max_tries)
self.comm.start()
else:
self.comm = SyncComm(port, timeout)
except CommError as e:
raise ControllerError(e)
self.command_index = random.randrange(len(string.printable))
self.command_i = string.printable[self.command_index]
self.motor_speed = [0, 0]
self.body_led = False
self.front_led = False
self.leds = 8 * [False]
self.logger = logging.getLogger('Controller')
def _binary_command(self, char):
"""Translate char to -char."""
return chr(256 - ord(char))
@command
def set_speed(self, left, right, callback=lambda x: x):
"""Set the speed of the motors."""
if (-self.MAX_SPEED <= left <= self.MAX_SPEED) \
and (-self.MAX_SPEED <= right <= self.MAX_SPEED):
command = "D%c,%d,%d\n" % (self.command_i, left, right)
ret = self.comm.send_command(command, self.command_i, 'd', callback)
return ret
else:
raise WrongCommand("Speed is out of bounds.")
@command
def get_speed(self, callback=lambda x: x):
"""Get speed of motors.
The speed is measured in pulses per second, one pulse is
aproximately 0.13mm. The speed can be in range
(-MAX_SPEED, MAX_SPEED).
Returns tuple (left motor speed, right motor speed).
"""
def _parse_response(response):
try:
left, right = response.strip().split(',')
return callback((int(left), int(right)))
except Exception as e:
self.logger.error(e)
raise ControllerError(e)
command = "E%c\n" % self.command_i
ret = self.comm.send_command(command, self.command_i, 'e', _parse_response)
return ret
@command
def set_body_led(self, value, callback=lambda x: x):
"""Set the green body LED's status.
Arguments:
value - boolean, turn the led on.
"""
command = "B%c,%d\n" % (self.command_i, 1 if value else 0)
ret = self.comm.send_command(command, self.command_i, 'b', callback)
return ret
@command
def set_front_led(self, value, callback=lambda x: x):
"""Set the bright front LED's status.
Arguments:
value - boolean, turn the led on.
"""
command = "F%c,%d\n" % (self.command_i, 1 if value else 0)
ret = self.comm.send_command(command, self.command_i, 'f', callback)
return ret
@command
def set_leds(self, value, callback=lambda x: x):
"""Set the all LEDs with one command.
Arguments:
value - boolean, turn the leds on.
"""
command = "L%c,9,%d\n" % (self.command_i, 1 if value else 0)
ret = self.comm.send_command(command, self.command_i, 'l', callback)
return ret
@command
def set_led(self, led_no, value, callback=lambda x: x):
"""Set the LED's status.
There are 8 LEDs on the e-puck robot. The LED number 0 is the frontal
one, the LED numbering is increasing clockwise.
Arguments:
led_no - number of the led (0 - 7).
value - boolean, turn the led on.
"""
if (0 <= led_no <= 7):
command = "L%c,%d,%d\n" % (self.command_i, led_no, 1 if value else 0)
ret = self.comm.send_command(command, self.command_i, 'l', callback)
return ret
else:
raise WrongCommand("Led number is out of the bounds.")
@command
def get_turning_selector(self, callback=lambda x: x):
"""Get the position of the rotating 16 positions selector.
Position 0 correspond to the arrow pointing on the right when looking
in the same direction as the robot.
"""
def _parse_response(response):
try:
value = response.strip()
return callback(int(value))
except Exception as e:
self.logger.error(e)
raise ControllerError(e)
command = "C%c\n" % self.command_i
ret = self.comm.send_command(command, self.command_i, 'c', _parse_response)
return ret
@command
def get_proximity_sensors(self, callback=lambda x: x):
"""Get the values of the 8 proximity sensors.
The 12 bit values of the 8 proximity sensors. For left and right side
there is one sensor situated 10 degrees from the front, others are 45
degrees and 90 degrees from the front. For each side there is also one
sensor on the back side.
The keys for sensor values are following: L10, L45, L90, LB, R10, R45,
R90, RB.
The values are in range [0, 4095].
"""
def _parse_response(response):
try:
r = map(int, response.split(','))
if len(r) != 8:
raise ControllerError("Wrong response")
return callback(dict(zip(['R10', 'R45', 'R90', 'RB', 'LB', 'L90', 'L45',
'L10'], r)))
except ValueError as e:
self.logger.error(e)
raise ControllerError(e)
command = "N%c\n" % self.command_i
ret = self.comm.send_command(command, self.command_i, 'n', _parse_response)
return ret
@command
def get_ambient_sensors(self, callback=lambda x: x):
"""Get the values of the 8 ambient light sensors.
The 12 bit values of the 8 ambient light sensors. For left and right
side there is one sensor situated 10 degrees from the front, others are
45 degrees and 90 degrees from the front. For each side there is also
one sensor on the back side.
The keys for sensor values are following: L10, L45, L90, LB, R10, R45,
R90, RB.
The values are in range [0, 4095].
"""
def _parse_response(response):
try:
r = map(int, response.split(','))
if len(r) != 8:
raise ControllerError("Wrong response")
return callback(dict(zip(['R10', 'R45', 'R90', 'RB', 'LB', 'L90', 'L45',
'L10'], r)))
except ValueError as e:
self.logger.error(e)
raise ControllerError(e)
command = "O%c\n" % self.command_i
ret = self.comm.send_command(command, self.command_i, 'o', _parse_response)
return ret
@command
def set_camera(self, mode, width, height, zoom, callback=lambda x: x):
"""Set the camera properties.
If the common denominator of zoom factor is 4 or 2, part of the
subsampling is done by the camera ( QQVGA = 4, QVGA = 2 ). This
increase the framerate by respectively 4 or 2. Moreover greyscale is
twice faster than color mode.
Arguments:
mode - GREYSCALE_MODE or RGB565_MODE
width - image width
height - image height
zoom - zoom factor
"""
if 0 < width <= 640 and 0 < height <= 480 and mode in (self.GREYSCALE_MODE, self.RGB565_MODE):
command = "J%c,%d,%d,%d,%d\n" % (self.command_i, mode, width, height, zoom)
ret = self.comm.send_command(command, self.command_i, 'j', callback)
return ret
else:
raise WrongCommand("Wrong camera properties.")
@command
def get_camera(self, callback=lambda x: x):
"""Get the camera properties.
Returns a dictionary with camera properties. The properites are:
mode - GREYSCALE_MODE or RGB565_MODE
width - image width
height - image height
zoom - zoom factor
"""
def _parse_response(response):
try:
r = map(int, response.strip().split(','))
return callback(dict(zip(['mode', 'width', 'height', 'zoom'], r)))
except ValueError as e:
self.logger.error(e)
raise ControllerError(e)
command = "I%c\n" % self.command_i
ret = self.comm.send_command(command, self.command_i, 'i', _parse_response)
return ret
@command
def get_photo(self, callback=lambda x: x):
"""Take a photo."""
def _parse_response(response):
mode = ord(response[0])
width = ord(response[1]) + (ord(response[2]) << 8);
height = ord(response[3]) + (ord(response[4]) << 8);
image = response[5:]
if mode == self.RGB565_MODE:
ret = ""
data_struct = "<BBB"
for (hi, lo) in zip(image[0::2], image[1::2]):
val = (ord(hi) << 8) + ord(lo)
b = int(((val >> 11) & 31) / 31. * 255)
g = int(((val >> 5) & 63) / 63. * 255)
r = int((val & 31) / 31. * 255)
ret += struct.pack(data_struct, b, g, r)
return callback(Image.fromstring('RGB', (width, height), ret).rotate(90))
elif mode == self.GREYSCALE_MODE:
return callback(Image.fromstring('L', (width, height), image).rotate(90))
c = self._binary_command("I")
command = "%c%c%c" % (c, self.command_i, chr(0))
ret = self.comm.send_command(command, self.command_i, c, _parse_response)
return ret
@command
def reset(self, callback=lambda x: x):
"""Reset the robot."""
command = "R%c\n" % self.command_i
ret = self.comm.send_command(command, self.command_i, 'r', callback)
return ret
@command
def set_motor_pos(self, left, right, callback=lambda x: x):
"""Set motor position.
The robot has two step motors. It is possible to set initial positions
of the motors (two numbers) and then every step of the motor increase
or decrease the position.
Note: This command doesn't alter the position of motors.
Arguments:
left - position of left motor
right - position of right motor
"""
command = "P%c,%d,%d\n" % (self.command_i, left, right)
ret = self.comm.send_command(command, self.command_i, 'p', callback)
return ret
@command
def get_motor_pos(self, callback=lambda x: x):
"""Read motor position.
Returns two values, position of left and right motor.
"""
def _parse_response(response):
try:
r = map(int, response.strip().split(','))
return callback(r)
except ValueError as e:
self.logger.error(e)
raise ControllerError(e)
command = "Q%c\n" % (self.command_i)
ret = self.comm.send_command(command, self.command_i, 'q', _parse_response)
return ret
@command
def get_raw_accelerometer(self, callback=lambda x: x):
"""Read accelerometer data.
Accelerometer measures acceleration in three axis (x, y, z).
Returns data as a dict with three keys: 'x', 'y' and 'z'.
"""
def _parse_response(response):
try:
r = dict(zip(['x', 'y', 'z'], map(int, response.strip().split(','))))
return callback(r)
except ValueError as e:
self.logger.error(e)
raise ControllerError(e)
command = "A%c\n" % (self.command_i)
ret = self.comm.send_command(command, self.command_i, 'a', _parse_response)
return ret
@command
def get_accelerometer(self, callback=lambda x: x):
"""Read the acceleration vector in spherical coords.
Three values can be computed from the acceleration sensors:
acceleration, orientation and inclination. The returned value is a dict
with these three keys.
Keys:
acceleration - length of the vector = intensity of the acceleration
inclination - inclination angle from the horizontal plane
0° = e-puck horizontal
90° = e-puck vertical
180° = e-puck horizontal but upside down
orientation - orientation of the acceleration in the horizontal
plane, zero facing front
0° = front part lower than rear part
90° = left part lower than right part
180° = rear part lower than front part
270° = right part lower than left part
"""
def _parse_response(response):
try:
acceleration = struct.unpack('<f', response[:4])[0]
orientation = struct.unpack('<f', response[4:8])[0]
inclination = struct.unpack('<f', response[8:12])[0]
return callback({'acceleration': acceleration, 'orientation':
orientation, 'inclination': inclination})
except struct.error as e:
self.logger.error(e)
raise ControllerError(e)
c = self._binary_command("A")
command = "%c%c%c" % (c, self.command_i, chr(0))
ret = self.comm.send_command(command, self.command_i, c, _parse_response)
return ret
@command
def calibrate_sensors(self, callback=lambda x: x):
"""Calibrate proximity sensors.
Remove any objects in sensors range.
"""
command = "K%c\n" % (self.command_i)
ret = self.comm.send_command(command, self.command_i, 'k', callback)
return ret
@command
def stop(self, callback=lambda x: x):
"""Stop the robot.
Stop the motors and turn off all leds.
"""
command = "S%c\n" % (self.command_i)
ret = self.comm.send_command(command, self.command_i, 's', callback)
return ret
@command
def play_sound(self, sound_no, callback=lambda x: x):
"""Play sound.
The robot is capable of playing 5 sounds, their numbers are:
1. "haa"
2. "spaah"
3. "ouah"
4. "yaouh"
5. "wouaaaaaaaah"
Any other number will turn the sound system off (and get rid of the
white noise).
"""
command = "T%c,%d\n" % (self.command_i, sound_no)
ret = self.comm.send_command(command, self.command_i, 't', callback)
return ret
@command
def get_volume(self, callback=lambda x: x):
"""Read volumes from microphones.
There are three microphones on the top of the robot.
The placement of microphones:
MIC 0 -- right side
MIC 1 -- left side
MIC 2 -- back
The returned value is a tuple of three volumes.
"""
def _parse_response(response):
try:
r = dict(zip(['R', 'L', 'B'], map(int, response.strip().split(','))))
return callback(r)
except ValueError as e:
self.logger.error(e)
raise ControllerError(e)
command = "U%c\n" % (self.command_i)
ret = self.comm.send_command(command, self.command_i, 'u', _parse_response)
return ret
@command
def get_microphone(self, on, callback=lambda x: x):
"""Perform FFT on data from microphones and return the results.
Arguments:
on -- turn recording on/off
"""
def _parse_response(response):
return [ord(x) + ord(y) * 1j for x,y in zip(response[::2], response[1::2])]
d = {'command': self._binary_command('Z'), 'timestamp': self.command_i,
'on': '1' if on else '0'}
command = "%(command)c%(timestamp)c%(on)c\x00" % d
ret = self.comm.send_command(command, self.command_i, d['command'], _parse_response)
return ret