def __init__(self,
bus_num=ThunderBorg.DEFAULT_BUS_NUM,
address=ThunderBorg.DEFAULT_I2C_ADDRESS,
log_level=logging.INFO,
debug=False):
self._debug = debug
log_level = logging.DEBUG if debug else log_level
cl = ConfigLogger()
cl.config(logger_name=self._BASE_LOGGER_NAME,
file_path=self._LOG_PATH,
level=log_level)
self._log = logging.getLogger(self._LOGGER_NAME)
if not self._debug:
self._tb = ThunderBorg(bus_num=bus_num,
address=address,
logger_name=self._TBORG_LOGGER_NAME,
log_level=log_level)
PYGameController.__init__(self,
logger_name=self._CTRL_LOGGER_NAME,
log_level=log_level,
debug=debug)
Daemon.__init__(self,
self._PIDFILE,
logger_name=self._LOGGER_NAME,
verbose=2 if debug else 0)
def test_set_i2c_address_with_address_range_invalid(self):
"""
Test that an exception is raised when the address is out of
range.
"""
new_addr = 0x78
with self.assertRaises(ThunderBorgException) as cm:
ThunderBorg.set_i2c_address(new_addr)
def test_config_with_auto_set_address(self):
"""
Test that ``auto_set_addr`` if set to ``True`` causes the API to
find the first valid board and configure itself to that board.
"""
# First change the board address so it cannot be found at the
# default address.
new_addr = 0x70
ThunderBorg.set_i2c_address(new_addr)
# Now instantiate ThunderBorg.
tb = ThunderBorg(logger_name=self._LOG_FILENAME,
log_level=logging.DEBUG,
auto_set_addr=True)
def test_set_i2c_address_without_current_address(self):
"""
Test that the ThunderBorg.set_i2c_address() can set a different
address. Scans address range to find current address.
"""
# Set a new address
new_addr = 0x70
ThunderBorg.set_i2c_address(new_addr)
found = ThunderBorg.find_board()
found = found[0] if found else 0
msg = "Found address '0x{:02X}', should be '0x{:02X}'.".format(
found, new_addr)
self.assertEqual(found, new_addr, msg)
def test_set_i2c_address_with_current_address(self):
"""
Test that the ThunderBorg.set_i2c_address() can set a different
address. The current address is provided.
"""
# Set a new address
new_addr = 0x70
cur_addr = ThunderBorg.DEFAULT_I2C_ADDRESS
ThunderBorg.set_i2c_address(new_addr, cur_addr=cur_addr)
found = ThunderBorg.find_board()
found = found[0] if found else 0
msg = "Found address '0x{:02X}', should be '0x{:02X}'.".format(
found, new_addr)
self.assertEqual(found, new_addr, msg)
def test_find_address_with_invalid_default_address(self):
"""
Test that an invalid default address will cause a board to be
initialized if the `auto_set_addr` argument is `True`.
"""
default_address = 0x15
# Initialize the board by instantiating ThunderBorg.
tb = ThunderBorg(logger_name=self.LOGGER_NAME,
log_level=logging.DEBUG,
auto_set_addr=True)
boards = ThunderBorg.find_board()
msg = "Boards found: {}".format(boards)
self.assertEquals(ThunderBorg.DEFAULT_I2C_ADDRESS, 0x20, msg)
self.assertTrue(len(boards) > 0, msg)
self.assertEqual(boards[0], default_address, msg)
def test_config_with_invalid_board_id(self):
"""
Test that an invalid board ID causes the expected exception.
"""
with self.assertRaises(ThunderBorgException) as cm:
ThunderBorg(logger_name=self._LOG_FILENAME,
log_level=logging.DEBUG)
def test_config_with_invalid_address(self):
"""
Test that an invalid address creates the expected exception.
"""
with self.assertRaises(ThunderBorgException) as cm:
ThunderBorg(address=0x70,
logger_name=self._LOG_FILENAME,
log_level=logging.DEBUG)
def test_find_board(self):
"""
Test that the ThunderBorg.find_board() method finds a board.
"""
found = ThunderBorg.find_board()
found = found[0] if found else 0
msg = "Found address '0x{:02X}', should be '0x{:02X}'.".format(
found, ThunderBorg.DEFAULT_I2C_ADDRESS)
self.assertEqual(found, ThunderBorg.DEFAULT_I2C_ADDRESS, msg)
def __init__(self,
bus_num=ThunderBorg.DEFAULT_BUS_NUM,
address=ThunderBorg.DEFAULT_I2C_ADDRESS,
borg=True,
log_level=logging.INFO,
voltage_in=_VOLTAGE_IN,
debug=False):
self._borg = borg
self.voltage_in = float(voltage_in)
log_level = logging.DEBUG if debug else log_level
cl = ConfigLogger()
cl.config(logger_name=self._BASE_LOGGER_NAME,
file_path=self._LOG_PATH,
level=log_level)
self._log = logging.getLogger(self._LOGGER_NAME)
super(JoyStickControl, self).__init__(self._PIDFILE,
logger_name=self._LOGGER_NAME)
self._log.info(
"Initial arguments: bus_num: %s, address: %s, "
"borg: %s, log_level: %s, voltage_in: %s, debug: %s", bus_num,
address, borg, logging.getLevelName(log_level), voltage_in, debug)
if self._borg:
self._tb = ThunderBorg(bus_num=bus_num,
address=address,
logger_name=self._TBORG_LOGGER_NAME,
log_level=log_level)
if math.isclose(self.voltage_in, 0.0):
self.voltage_in = self._tb.get_battery_voltage()
self._log.info("Voltage in: %s, max power: %s", self.voltage_in,
self.max_power)
self.set_battery_limits()
# Set defaults
self.__quit = False
self.fwd_rev_invert = False
self.turn_invert = False
# Longer than 10 secs will never be recognized because the
# controller itself will disconnect before that.
self.quit_hold_time = 9.0
def setUpClass(self):
ThunderBorg.DEFAULT_I2C_ADDRESS = 0x15
ThunderBorg.set_i2c_address(ThunderBorg.DEFAULT_I2C_ADDRESS)
tb = ThunderBorg()
tb.halt_motors()
tb.set_both_leds(0, 0, 0)
tb.set_led_battery_state(False)
tb.set_comms_failsafe(False)
tb.write_external_led_word(0, 0, 0, 0)
def setUp(self):
self._tb = ThunderBorg(logger_name=self._LOG_FILENAME,
log_level=logging.DEBUG)
class TestThunderBorg(BaseTest):
_LOG_FILENAME = 'tb-instance.log'
def __init__(self, name):
super(TestThunderBorg, self).__init__(
name, filename=self._LOG_FILENAME)
def setUp(self):
self._tb = ThunderBorg(logger_name=self._LOG_FILENAME,
log_level=logging.DEBUG)
def tearDown(self):
self._tb.halt_motors()
self._tb.set_comms_failsafe(False)
self._tb.set_led_battery_state(False)
self._tb.set_led_one(0.0, 0.0, 0.0)
self._tb.set_led_two(0.0, 0.0, 0.0)
self._tb.set_battery_monitoring_limits(7.0, 36.3)
self._tb.write_external_led_word(0, 0, 0, 0)
self._tb.close_streams()
def validate_tuples(self, t0, t1):
msg = "rgb0: {:0.2f}, rgb1: {:0.2f}"
for x, y in zip(t0, t1):
self.assertAlmostEqual(x, y, delta=0.01, msg=msg.format(x, y))
#@unittest.skip("Temporarily skipped")
def test_set_and_get_motor_one(self):
"""
Test that motor one responds to commands.
"""
# Test forward
speeds = (0.0, 0.25, 0.5, 0.75, 1.0, 1.25)
for speed in speeds:
self._tb.set_motor_one(speed)
rcvd_speed = self._tb.get_motor_one()
msg = "Speed sent: {}, speed received: {}".format(
speed, rcvd_speed)
self.assertLessEqual(rcvd_speed, 1.0, msg=msg)
time.sleep(1.0)
# Test reverse
speeds = (-0.0 -0.25, -0.5, -0.75, -1.0, -1.25)
for speed in speeds:
self._tb.set_motor_one(speed)
rcvd_speed = self._tb.get_motor_one()
msg = "Speed sent: {}, speed received: {}".format(
speed, rcvd_speed)
self.assertGreaterEqual(rcvd_speed, -1.0, msg=msg)
time.sleep(1.0)
#@unittest.skip("Temporarily skipped")
def test_set_and_get_motor_two(self):
"""
Test that motor two responds to commands.
"""
# Test forward
speeds = (0.0, 0.25, 0.5, 0.75, 1.0, 1.25)
for speed in speeds:
self._tb.set_motor_two(speed)
rcvd_speed = self._tb.get_motor_two()
msg = "Speed sent: {}, speed received: {}".format(
speed, rcvd_speed)
self.assertLessEqual(rcvd_speed, 1.0, msg=msg)
time.sleep(1.0)
# Test reverse
speeds = (-0.0 -0.25, -0.5, -0.75, -1.0, -1.25)
for speed in speeds:
self._tb.set_motor_two(speed)
rcvd_speed = self._tb.get_motor_two()
msg = "Speed sent: {}, speed received: {}".format(
speed, rcvd_speed)
self.assertGreaterEqual(rcvd_speed, -1.0, msg=msg)
time.sleep(1.0)
#@unittest.skip("Temporarily skipped")
def test_set_both_motors(self):
"""
Test that motors one and two responds to commands.
"""
# Test forward
speed = 0.5
self._tb.set_both_motors(speed)
rcvd_speed = self._tb.get_motor_one()
msg = "Speed sent: {}, speed received: {}".format(speed, rcvd_speed)
self.assertAlmostEqual(speed, rcvd_speed, delta=0.01, msg=msg)
rcvd_speed = self._tb.get_motor_two()
msg = "Speed sent: {}, speed received: {}".format(speed, rcvd_speed)
self.assertAlmostEqual(speed, rcvd_speed, delta=0.01, msg=msg)
# Test reverse
speed = -0.5
self._tb.set_both_motors(speed)
rcvd_speed = self._tb.get_motor_one()
msg = "Speed sent: {}, speed received: {}".format(speed, rcvd_speed)
self.assertAlmostEqual(speed, rcvd_speed, delta=0.01, msg=msg)
rcvd_speed = self._tb.get_motor_two()
msg = "Speed sent: {}, speed received: {}".format(speed, rcvd_speed)
self.assertAlmostEqual(speed, rcvd_speed, delta=0.01, msg=msg)
#@unittest.skip("Temporarily skipped")
def test_halt_motors(self):
"""
Test that halting the motors works properly.
"""
# Start motors and check that the board says they are moving.
speed = 0.5
self._tb.set_both_motors(speed)
rcvd_speed = self._tb.get_motor_one()
msg = "Speed sent: {}, speed received: {}".format(speed, rcvd_speed)
self.assertAlmostEqual(speed, rcvd_speed, delta=0.01, msg=msg)
rcvd_speed = self._tb.get_motor_two()
msg = "Speed sent: {}, speed received: {}".format(speed, rcvd_speed)
self.assertAlmostEqual(speed, rcvd_speed, delta=0.01, msg=msg)
# Halt the motors.
self._tb.halt_motors()
# Check that the board says they are not moving.
speed = 0.0
rcvd_speed = self._tb.get_motor_one()
msg = "Speed sent: {}, speed received: {}".format(speed, rcvd_speed)
self.assertAlmostEqual(speed, rcvd_speed, delta=0.01, msg=msg)
rcvd_speed = self._tb.get_motor_two()
msg = "Speed sent: {}, speed received: {}".format(speed, rcvd_speed)
self.assertAlmostEqual(speed, rcvd_speed, delta=0.01, msg=msg)
#@unittest.skip("Temporarily skipped")
def test_set_get_led_one(self):
"""
Test that the RBG colors set are the same as the one's returned.
"""
state = self._tb.get_led_battery_state()
msg = "Default state should be False: {}".format(state)
self.assertFalse(state, msg)
rgb_list = [(0, 0, 0), (1, 1, 1), (1.0, 0.5, 0.0), (0.2, 0.0, 0.2)]
for rgb in rgb_list:
self._tb.set_led_one(*rgb)
ret_rgb = self._tb.get_led_one()
self.validate_tuples(ret_rgb, rgb)
#@unittest.skip("Temporarily skipped")
def test_set_get_led_two(self):
"""
Test that the RBG colors set are the same as the one's returned.
"""
state = self._tb.get_led_battery_state()
msg = "Default state should be False: {}".format(state)
self.assertFalse(state, msg)
rgb_list = [(0, 0, 0), (1, 1, 1), (1.0, 0.5, 0.0), (0.2, 0.0, 0.2)]
for rgb in rgb_list:
self._tb.set_led_two(*rgb)
ret_rgb = self._tb.get_led_two()
self.validate_tuples(ret_rgb, rgb)
#@unittest.skip("Temporarily skipped")
def test_set_both_leds(self):
"""
Test that the RBG colors set are the same as the one's returned.
"""
state = self._tb.get_led_battery_state()
msg = "Default state should be False: {}".format(state)
self.assertFalse(state, msg)
rgb_list = [(0, 0, 0), (1, 1, 1), (1.0, 0.5, 0.0), (0.2, 0.0, 0.2)]
for rgb in rgb_list:
self._tb.set_both_leds(*rgb)
ret_rgb = self._tb.get_led_one()
self.validate_tuples(ret_rgb, rgb)
ret_rgb = self._tb.get_led_two()
self.validate_tuples(ret_rgb, rgb)
#@unittest.skip("Temporarily skipped")
def test_set_and_get_led_battery_state(self):
"""
Test that the LED state changes.
"""
state = self._tb.get_led_battery_state()
msg = "Default state should be False: {}".format(state)
self.assertFalse(state, msg)
# Change the state of the LEDs to monitor the batteries.
self._tb.set_led_battery_state(True)
state = self._tb.get_led_battery_state()
msg = "Battery monitoring state should be True".format(state)
self.assertTrue(state, msg)
#@unittest.skip("Temporarily skipped")
def test_set_and_get_comms_failsafe(self):
"""
Test that the failsafe changes states.
"""
failsafe = self._tb.get_comms_failsafe()
msg = "Default failsafe should be False: {}".format(failsafe)
self.assertFalse(failsafe, msg)
# Test that motors run continuously.
speed = 0.5
self._tb.set_both_motors(speed)
sleep = 1 # Seconds
time.sleep(sleep)
msg = "Motors should run for {} second.".format(sleep)
m0_speed = self._tb.get_motor_one()
m1_speed = self._tb.get_motor_two()
self.assertAlmostEqual(m0_speed, speed, delta=0.1, msg=msg)
self.assertAlmostEqual(m1_speed, speed, delta=0.1, msg=msg)
self._tb.halt_motors()
# Turn on failsafe
self._tb.set_comms_failsafe(True)
failsafe = self._tb.get_comms_failsafe()
msg = "Failsafe should be True: {}".format(failsafe)
self.assertTrue(failsafe, msg)
# Start up motors
self._tb.set_both_motors(speed)
time.sleep(sleep)
msg = ("Motors should run for 1/4 of a second with sleep of {} "
"second(s).").format(sleep)
m0_speed = self._tb.get_motor_one()
m1_speed = self._tb.get_motor_two()
self.assertNotAlmostEqual(m0_speed, speed, delta=0.1, msg=msg)
self.assertNotAlmostEqual(m1_speed, speed, delta=0.1, msg=msg)
# Send keepalive
msg = "Motors are running for {} seconds"
interval = 0.25
sleep = 1.5 # Seconds
for itr in range(6):
self._tb.set_motor_one(0.5)
self._tb.set_motor_two(0.5)
time.sleep(interval)
m0_speed = self._tb.get_motor_one()
m1_speed = self._tb.get_motor_two()
t = (itr + 1) * interval
self.assertAlmostEqual(m0_speed, speed, delta=0.1,
msg=msg.format(t))
self.assertAlmostEqual(m1_speed, speed, delta=0.1,
msg=msg.format(t))
#@unittest.skip("Temporarily skipped")
def test_get_drive_fault_one(self):
"""
Test that `get_drive_fault_one` returns data.
"""
fault = self._tb.get_drive_fault_one()
msg = "Fault value should be False, found: {}"
self.assertFalse(fault, msg.format(fault))
# Run motor one for a second.
speed = 0.5
sleep = 1 # Seconds
self._tb.set_motor_one(speed)
self._tb.halt_motors()
# Test that fault is cleared.
fault = self._tb.get_drive_fault_one()
self.assertFalse(fault, msg.format(fault))
#@unittest.skip("Temporarily skipped")
def test_get_drive_fault_two(self):
"""
Test that `get_drive_fault_two` returns data.
"""
fault = self._tb.get_drive_fault_two()
msg = "Fault value should be False, found: {}"
self.assertFalse(fault, msg.format(fault))
# Run motor one for a second.
speed = 0.5
sleep = 1 # Seconds
self._tb.set_motor_two(speed)
self._tb.halt_motors()
# Test that fault is cleared.
fault = self._tb.get_drive_fault_two()
self.assertFalse(fault, msg.format(fault))
#@unittest.skip("Temporarily skipped")
def test_get_battery_voltage(self):
"""
Test that the battery voltage is in range.
"""
vmin = ThunderBorg._BATTERY_MIN_DEFAULT
vmax = ThunderBorg._BATTERY_MAX_DEFAULT
voltage = self._tb.get_battery_voltage()
msg = ("Voltage should be in the range of {:0.02f} to {:0.02f}, "
"found {:0.02f} volts").format(vmin, vmax, voltage)
self.assertTrue(vmin <= voltage <= vmax, msg)
#@unittest.skip("Temporarily skipped")
def test_set_get_battery_monitoring_limits(self):
"""
Test that setting and getting the battery monitoring limits
functions properly.
.. note::
Set limits based on a fully charged LiIon battery pack. This
could be anywhere between 15.4 and 16.8 volts maximum depending
on the type of batteries used. The minimum should never be less
than 3 volts per cell or in a four pack 12 volts.
"""
vmin = 12.0
vmax = 16.8
self._tb.set_battery_monitoring_limits(vmin, vmax)
voltage = self._tb.get_battery_voltage()
minimum, maximum = self._tb.get_battery_monitoring_limits()
msg = ("Found minimum {:0.2f} and maximum {:0.2f} volts, should be "
"minimum {:0.2f} and maximum {:0.2f} volts, actual voltage "
"{:0.2f}").format(minimum, maximum, vmin, vmax, voltage)
self.assertAlmostEqual(minimum, vmin, delta=0.1, msg=msg)
self.assertAlmostEqual(maximum, vmax, delta=0.1, msg=msg)
# Check that the actual voltage is within the above ranges.
self.assertTrue(vmin <= voltage <= vmax, msg)
@unittest.skip("Temporarily skipped")
def test_write_external_led_word(self):
"""
Test that writing binary data with the `write_external_led_word`
method sets the LED correctly.
"""
# Check that both LEDs are off.
self._tb.write_external_led_word(0, 0, 0, 0)
# Turn on both LEDs.
color = (255, 64, 1, 0)
self._tb.write_external_led_word(*color)
#self.validate_tuples(led2, off)
@unittest.skip("Temporarily skipped")
def test_set_external_led_colors(self):
"""
Test that setting external LEDs works correctly.
"""
# Set a single LED to yellow.
yellow = [[1.0, 1.0, 0.0]]
self._tb.set_external_led_colors(yellow)
def tearDown(self):
ThunderBorg.set_i2c_address(ThunderBorg.DEFAULT_I2C_ADDRESS)
class JoyStickControl(Daemon):
"""
The ApproxEng.input library is used to control the Thunder Borg.
"""
_LOG_PATH = os.path.join(LOG_PATH, 'mborg_approxeng.log')
_BASE_LOGGER_NAME = 'examples'
_LOGGER_NAME = 'examples.mborg-approxeng'
_TBORG_LOGGER_NAME = 'examples.tborg'
_PIDFILE = os.path.join(RUN_PATH, 'mborg_approxeng.pid')
_VOLTAGE_IN = 12 # 1.2 volt cells * 10
_VOLTAGE_OUT = 12.0 * 0.95
_MAX_VOLTAGE_MULT = 1.145
_ROTATE_TURN_SPEED = 0.5
_SLOW_SPEED = 0.5
def __init__(self,
bus_num=ThunderBorg.DEFAULT_BUS_NUM,
address=ThunderBorg.DEFAULT_I2C_ADDRESS,
borg=True,
log_level=logging.INFO,
voltage_in=_VOLTAGE_IN,
debug=False):
self._borg = borg
self.voltage_in = float(voltage_in)
log_level = logging.DEBUG if debug else log_level
cl = ConfigLogger()
cl.config(logger_name=self._BASE_LOGGER_NAME,
file_path=self._LOG_PATH,
level=log_level)
self._log = logging.getLogger(self._LOGGER_NAME)
super(JoyStickControl, self).__init__(self._PIDFILE,
logger_name=self._LOGGER_NAME)
self._log.info(
"Initial arguments: bus_num: %s, address: %s, "
"borg: %s, log_level: %s, voltage_in: %s, debug: %s", bus_num,
address, borg, logging.getLevelName(log_level), voltage_in, debug)
if self._borg:
self._tb = ThunderBorg(bus_num=bus_num,
address=address,
logger_name=self._TBORG_LOGGER_NAME,
log_level=log_level)
if math.isclose(self.voltage_in, 0.0):
self.voltage_in = self._tb.get_battery_voltage()
self._log.info("Voltage in: %s, max power: %s", self.voltage_in,
self.max_power)
self.set_battery_limits()
# Set defaults
self.__quit = False
self.fwd_rev_invert = False
self.turn_invert = False
# Longer than 10 secs will never be recognized because the
# controller itself will disconnect before that.
self.quit_hold_time = 9.0
@property
def max_power(self):
return (1.0 if self._VOLTAGE_OUT > self.voltage_in else
self._VOLTAGE_OUT / self.voltage_in)
def set_battery_limits(self):
max_level = self.voltage_in * self._MAX_VOLTAGE_MULT
if 7.0 <= self.voltage_in < 12: # 9 volt battery
min_level = 7.5
elif 12 <= self.voltage_in < 13.6: # 10 NIMH 1.2 volt batteries
min_level = 9.5
elif 13.6 <= self.voltage_in < 17.6: # 4 LiIon 3.6 volt batteries
min_level = 12.0
else:
min_level = self.voltage_in
self._log.error("Could not determine battery type.")
self._tb.set_battery_monitoring_limits(min_level, max_level)
def run(self):
"""
Start the controller listening process.
"""
if self._borg:
# Turn on failsafe.
self._tb.set_comms_failsafe(True)
if self._tb.get_comms_failsafe():
# Log and init
self.log_battery_monitoring()
self.init_mborg()
else:
self._log.error("The failsafe mode could not be turned on.")
self.quit = True
try:
self.listen()
except (KeyboardInterrupt, ThunderBorgException) as e:
self._log.warn("Exiting event processing, %s", e)
finally:
self._tb.halt_motors()
self._tb.set_comms_failsafe(False)
self._tb.set_led_battery_state(False)
self._tb.set_both_leds(0, 0, 0) # Set LEDs off
self._log.info("Exiting")
if self.quit: # Only shutdown if asked.
self._log.warn("Shutting down the Raspberry PI.")
os.system("sudo poweroff")
sys.exit()
def log_battery_monitoring(self):
"""
Dump to the log the initial battery values.
"""
level_min, level_max = self._tb.get_battery_monitoring_limits()
current_level = self._tb.get_battery_voltage()
mid_level = (level_min + level_max) / 2
buf = six.StringIO()
buf.write("\nBattery Monitoring Settings\n")
buf.write("---------------------------\n")
buf.write("Minimum (red) {:02.2f} V\n".format(level_min))
buf.write("Middle (yellow) {:02.2f} V\n".format(mid_level))
buf.write("Maximum (green) {:02.2f} V\n".format(level_max))
buf.write("Current Voltage {:02.2f} V\n".format(current_level))
self._log.info(buf.getvalue())
buf.close()
def init_mborg(self):
"""
Initialize motor controller.
"""
self._tb.halt_motors()
self._tb.set_led_battery_state(False)
self._tb.set_both_leds(0, 0, 1) # No joystick yet.
self._log.debug("Battery LED state: %s, LED color one: %s, two: %s ",
self._tb.get_led_battery_state(),
self._tb.get_led_one(), self._tb.get_led_two())
@property
def quit(self):
return self.__quit
@quit.setter
def quit(self, value):
self.__quit = value
@property
def quit_hold_time(self):
return self.__quit_hold_time
@quit_hold_time.setter
def quit_hold_time(self, value):
self.__quit_hold_time = value
@property
def fwd_rev_invert(self):
return self.__fwd_rev_invert
@fwd_rev_invert.setter
def fwd_rev_invert(self, value):
self.__fwd_rev_invert = value
@property
def turn_invert(self):
return self.__turn_invert
@turn_invert.setter
def turn_invert(self, value):
self.__turn_invert = value
def listen(self):
_mode = 0 # For now just set to 0
motor_one = motor_two = 0
while not self.quit:
try:
with ControllerResource() as joystick:
while joystick.connected and not self.quit:
if not self._tb.get_led_battery_state():
self._tb.set_led_battery_state(True)
## Set key presses
kp_map = self._check_presses(joystick)
# Set the mode.
_mode = kp_map['mode']
# Set the quit hold time.
quit_hold_time = kp_map['quit_hold_time']
if (not self.quit and self.quit_hold_time != 0
and self.quit_hold_time <=
math.floor(quit_hold_time)):
self.quit = True
# Set turning mode
turning_mode = kp_map['turning_mode']
# Set slow speed
slow_speed = kp_map['slow_speed']
## Set ly and rx axes
motor_one, motor_two, x = self._process_motion(
joystick, 'ly', 'rx')
# Set the drive slow speed.
x *= turning_mode
if x > 0.05:
motor_one *= 1.0 - (2.0 * x)
elif x < -0.05:
motor_two *= 1.0 + (2.0 * x)
motor_one *= slow_speed
motor_two *= slow_speed
if self._borg:
self._tb.set_motor_one(motor_one * self.max_power)
self._tb.set_motor_two(motor_two * self.max_power)
# Set LEDs to purple to indicate motor faults.
if ((self._tb.get_drive_fault_one()
or self._tb.get_drive_fault_two())
and self._tb.get_led_battery_state()):
self._tb.set_led_battery_state(False)
self._tb.set_both_leds(1, 0, 1) # purple
else:
time.sleep(0.25)
except IOError:
self._tb.set_both_leds(0, 0, 1) # Set to blue
self._log.debug("Waiting for controller")
time.sleep(2.0)
def _process_motion(self, joystick, fr, tn):
## Set forward/reverse and turning axes
axis_map = self._check_axes(joystick)
# Set left Y or pitch axis
fwd_rev = axis_map[fr]
# Set right X axis or roll
turn = axis_map[tn]
# Invert the forward/reverse axis to match motor.
if self.fwd_rev_invert:
motor_one = motor_two = -fwd_rev
else:
motor_one = motor_two = fwd_rev
# Invert the turning axis to match motor.
if self.turn_invert:
x = -turn
else:
x = turn
return motor_one, motor_two, x
def _check_presses(self, joystick):
kp_map = {
'mode': False,
'quit_hold_time': 0.0,
'slow_speed': 1,
'turning_mode': 1,
}
joystick.check_presses()
for button_name in joystick.buttons.names:
hold_time = joystick[button_name]
if hold_time is not None:
if button_name == 'home':
# Hold time for quit
kp_map['quit_hold_time'] = hold_time
if button_name == 'start':
# Change mode switch
kp_map['mode'] = True
if button_name == 'r1':
# Two wheel turning
kp_map['turning_mode'] = self._ROTATE_TURN_SPEED
if button_name == 'l1':
# Drive slow button press
kp_map['slow_speed'] = self._SLOW_SPEED
return kp_map
def _check_axes(self, joystick):
axis_map = {'ly': 0.0, 'rx': 0.0, 'pitch': 0.0, 'roll': 0.0}
for axis_name in joystick.axes.names:
axis_value = joystick[axis_name]
self._log.debug("%s: %s", axis_name, axis_value)
if axis_name == 'ly':
axis_map[axis_name] = float(axis_value)
elif axis_name == 'rx':
axis_map[axis_name] = float(axis_value)
elif axis_name == 'pitch':
axis_map[axis_name] = float(axis_value)
elif axis_name == 'roll':
axis_map[axis_name] = float(axis_value)
return axis_map
class JoyStickControl(PYGameController, Daemon):
"""
This class allows control of the MonsterBorg by a PS3/4 controller.
"""
_LOG_PATH = os.path.join(LOG_PATH, 'mborg_pygame.log')
_BASE_LOGGER_NAME = 'examples'
_LOGGER_NAME = 'examples.mborg-pygame'
_CTRL_LOGGER_NAME = 'examples.controller'
_TBORG_LOGGER_NAME = 'examples.tborg'
_PIDFILE = os.path.join(RUN_PATH, 'mborg_pygame.pid')
_VOLTAGE_IN = 1.2 * 10
_VOLTAGE_OUT = 12.0 * 0.95
_PROCESS_INTERVAL = 0.00
_MAX_POWER = (1.0 if _VOLTAGE_OUT > _VOLTAGE_IN else _VOLTAGE_OUT /
float(_VOLTAGE_IN))
_ROTATE_TURN_SPEED = 0.5
_SLOW_SPEED = 0.5
def __init__(self,
bus_num=ThunderBorg.DEFAULT_BUS_NUM,
address=ThunderBorg.DEFAULT_I2C_ADDRESS,
log_level=logging.INFO,
debug=False):
self._debug = debug
log_level = logging.DEBUG if debug else log_level
cl = ConfigLogger()
cl.config(logger_name=self._BASE_LOGGER_NAME,
file_path=self._LOG_PATH,
level=log_level)
self._log = logging.getLogger(self._LOGGER_NAME)
if not self._debug:
self._tb = ThunderBorg(bus_num=bus_num,
address=address,
logger_name=self._TBORG_LOGGER_NAME,
log_level=log_level)
PYGameController.__init__(self,
logger_name=self._CTRL_LOGGER_NAME,
log_level=log_level,
debug=debug)
Daemon.__init__(self,
self._PIDFILE,
logger_name=self._LOGGER_NAME,
verbose=2 if debug else 0)
def run(self):
"""
Start the controller listening process.
"""
self.init_controller()
if not self._debug:
# Turn on failsafe.
self._tb.set_comms_failsafe(True)
if self._tb.get_comms_failsafe():
self.log_battery_monitoring()
self.init_mborg()
else:
self._clog.error("The failsafe mode could not be turned on.")
self.set_quit()
try:
self.listen()
except (KeyboardInterrupt, ThunderBorgException) as e:
self._log.warn("Exiting event processing, %s", e)
except Exception as e:
self._log.error("Unknown error, %s", e, exc_info=True)
finally:
self._tb.halt_motors()
self._tb.set_comms_failsafe(False)
self._tb.set_led_battery_state(False)
self._tb.set_both_leds(0, 0, 0) # Set LEDs off
self._log.info("Exiting")
sys.exit()
def log_battery_monitoring(self):
"""
Dump to the log the initial battery values.
"""
level_min, level_max = self._tb.get_battery_monitoring_limits()
current_level = self._tb.get_battery_voltage()
mid_level = (level_min + level_max) / 2
buf = six.StringIO()
buf.write("\nBattery Monitoring Settings\n")
buf.write("---------------------------\n")
buf.write("Minimum (red) {:02.2f} V\n".format(level_min))
buf.write("Middle (yellow) {:02.2f} V\n".format(mid_level))
buf.write("Maximum (green) {:02.2f} V\n".format(level_max))
buf.write("Current Voltage {:02.2f} V\n".format(current_level))
self._log.info(buf.getvalue())
buf.close()
def init_mborg(self):
"""
Initialize the MonsterBorg joystick controller.
"""
self._tb.halt_motors()
self._tb.set_led_battery_state(False)
self._tb.set_both_leds(0, 0, 1) # Set to blue
self.event_wait_time = self._PROCESS_INTERVAL
if not self.is_ctrl_init:
self._log.warn("Could not initialize ")
self._tb.set_comms_failsafe(False)
self._tb.set_both_leds(0, 0, 0) # Set LEDs off
sys.exit()
self.set_defaults()
self._tb.set_led_battery_state(True)
self._led_battery_mode = True
self._log.debug("Finished mborg_joy initialization.")
def process_event(self):
"""
Process the current events (overrides the base class method).
"""
# Invert the controller Y axis to match the motor fwd/rev.
# If the Y axis needs to be inverted do that also.
if self.axis_y_invert:
motor_one = motor_two = self.axis_data.get(self.LF_UD)
else:
motor_one = motor_two = -self.axis_data.get(self.LF_UD)
if self.axis_x_invert:
x = -self.axis_data.get(self.RT_LR)
else:
x = self.axis_data.get(self.RT_LR)
# Rotate turn button press
if not self.button_data.get(self.rotate_turn_button):
x *= self.rotate_turn_speed
if x > 0.05:
motor_one *= 1.0 - (2.0 * x)
elif x < -0.05:
motor_two *= 1.0 + (2.0 * x)
# Drive slow button press
if self.button_data.get(self.drive_slow_button):
motor_one *= self.drive_slow_speed
motor_two *= self.drive_slow_speed
if not self._debug:
self._tb.set_motor_one(motor_one * self._MAX_POWER)
self._tb.set_motor_two(motor_two * self._MAX_POWER)
# Set LEDs to purple to indicate motor faults.
if (self._tb.get_drive_fault_one()
or self._tb.get_drive_fault_two()):
if self._led_battery_mode:
self._tb.set_led_battery_state(False)
self._tb.set_both_leds(1, 0, 1) # Set to purple
self._led_battery_mode = False
elif not self._led_battery_mode:
self._tb.set_led_battery_state(True)
self._led_battery_mode = True
def set_defaults(self, **kwargs):
"""
Set some default values. This method can be set while running. For
example if the robot flips over which could be determined with a
sensor the axis invert values can be changed.
:param axis_y_invert: If set to `True` the up/down control is
inverted. Default is `False`. Can be used
if the robot flips over.
:type axis_y_invert: bool
:param axis_x_invert: If set to `True` the left/right control is
inverted. Default is `False`. Can be used
if the robot flips over.
:type axis_x_invert: bool
:param rotate_turn_button: Choose the button for rotation. The
default is R1 (5).
:type rotate_turn_button: int
:param rotate_turn_speed: Choose the speed for rotation. The
default is 0.5.
:type rotate_turn_speed: float
:param drive_slow_button: Choose the button for driving slow. The
default is R2 (6).
:type drive_slow_but: int
:param drive_slow_speed: Choose the speed to decrease to when the
drive-slow button is held.
:type drive_slow_speed: bool
"""
tmp_kwargs = kwargs.copy()
self.axis_y_invert = tmp_kwargs.pop('axis_y_invert', False)
self.axis_x_invert = tmp_kwargs.pop('axis_x_invert', False)
self.rotate_turn_button = tmp_kwargs.pop('rotate_turn_button', self.R1)
self.rotate_turn_speed = tmp_kwargs.pop('rotate_turn_speed',
self._ROTATE_TURN_SPEED)
self.drive_slow_button = tmp_kwargs.pop('drive_slow_button', self.L1)
self.drive_slow_speed = tmp_kwargs.pop('drive_slow_speed',
self._SLOW_SPEED)
if kwargs:
self._log.error("Invalid arguments found: %s", kwargs)