def __init__(self, rail_cars=0):
self.config = lib.get_config()
self.PID_values = self.config["IR_PID"]
self.device = IR() # INSTANTIATE ONLY ONCE
self.north = Side("North Left", "North Right", self.device.read_values,
self.PID_values["North"]["diff"],
self.PID_values["North"]["dist"])
self.south = Side("South Left", "South Right", self.device.read_values,
self.PID_values["South"]["diff"],
self.PID_values["South"]["dist"])
self.east = Side("East Top", "East Bottom", self.device.read_values,
self.PID_values["East"]["diff"],
self.PID_values["East"]["dist"])
self.west = Side("West Top", "West Bottom", self.device.read_values,
self.PID_values["West"]["diff"],
self.PID_values["West"]["dist"])
self.driver = OmniDriver()
self.sides = {
"north": self.north,
"south": self.south,
"west": self.west,
"east": self.east
}
self.moving = False
self.logger = lib.get_logger()
mapping = ["EXIT", "west", "east", "EXIT"]
self.rail_cars_side = mapping[rail_cars]
def __init__(self, arm_config):
self.logger = lib.get_logger()
self.bot_config = lib.get_config()
self.servo_cape \
= ServoCape(self.bot_config["dagu_arm"]["servo_cape_arm"])
self.servo_cape_grabber \
= ServoCape(self.bot_config["dagu_arm"]["servo_cape_grabber"])
# QR scanning tools.
self.scanner = zbar.ImageScanner()
self.scanner.parse_config('enable')
# Figure out what camera is being used
cam_model = arm_config["camera"]
self.cam = Camera(self.bot_config[cam_model])
self.rail = Rail_Mover()
# initialize vertices of QR code
l = 1.5
self.qr_verts = np.float32([[-l/2, -l/2, 0],
[-l/2, l/2, 0],
[ l/2, -l/2, 0],
[ l/2, l/2, 0]])
# Angles of all of the joints.
# DO NOT SEND ANGLES ANY OTHER WAY
self.joints = HOME
self.hopper = [None, None, None, None]
self.IR = IR()
def __init__(self, gpio_pins):
"""Setup GPIO interface."""
# Build logger
self.logger = lib.get_logger()
# Load config
self.config = lib.get_config()
# This variable is inversely proportional to the speed
# of rotation. It is set to max speed by default.
self.time_int = 1.0
if self.config["test_mode"]["stepper"]:
# Get dir of simulated hardware files from config
gpio_test_dir = self.config['test_gpio_base_dir']
# Build gpio objects for BBB interaction, provide test dir
self.gpio_1 = gpio.GPIO(gpio_pins[0], gpio_test_dir)
self.gpio_2 = gpio.GPIO(gpio_pins[1], gpio_test_dir)
self.gpio_3 = gpio.GPIO(gpio_pins[2], gpio_test_dir)
self.gpio_4 = gpio.GPIO(gpio_pins[3], gpio_test_dir)
else:
# Build gpio pins
self.gpio_1 = gpio.GPIO(gpio_pins[0])
self.gpio_2 = gpio.GPIO(gpio_pins[1])
self.gpio_3 = gpio.GPIO(gpio_pins[2])
self.gpio_4 = gpio.GPIO(gpio_pins[3])
self.gpio_1.output()
self.gpio_2.output()
self.gpio_3.output()
self.gpio_4.output()
def __init__(self, cam_config):
self.logger = lib.get_logger()
udev_name = cam_config["udev_name"]
print udev_name
cam_num = find_name(udev_name)
#extract calib data from cam_config
self.a = cam_config["a"]
self.n = cam_config["n"]
self.cam = cv2.VideoCapture(cam_num)
self.cam.set(3, 1280)
self.cam.set(4, 720)
self.resX = int(self.cam.get(3))
self.resY = int(self.cam.get(4))
# QR scanning tools
self.scanner = zbar.ImageScanner()
self.scanner.parse_config('enable')
(self.grabbed, self.frame) = self.cam.read()
self.stopped = True
def __init__(self):
self.config = lib.get_config()
self.logger = lib.get_logger()
self.servo_pwm = self.config["rubiks"]["servo_pwm"]
self.rev_num = self.config["rubiks"]["GPIO"]["REV"]
self.fwd_num = self.config["rubiks"]["GPIO"]["FWD"]
self.pwr_num = self.config["rubiks"]["GPIO"]["PWR"]
# Build servo that controlls gripper that turns cube.
self.gripper = servo_mod.Servo(self.servo_pwm)
# Set to starting position
self.gripper.position = START_POSITION
# gpio's that control motors of gripper.
# Note: we're not using motor.py, it assumes strange hardware.
# TODO(AhmedSamara): update motor.py to update standard hardware.
self.rev = gpio_mod.GPIO(self.rev_num)
self.fwd = gpio_mod.GPIO(self.fwd_num)
self.pwr = gpio_mod.GPIO(self.pwr_num)
# set directions
self.pwr.output()
self.fwd.output()
self.rev.output()
# set initial value
self.pwr.set_value(0)
self.fwd.set_value(0)
self.rev.set_value(0)
def __init__(self, gpio_pins):
"""Setup GPIO interface."""
# Build logger
self.logger = lib.get_logger()
# Load config
self.config = lib.get_config()
if self.config["test_mode"]["micromotor"]:
# Get dir of simulated hardware files from config
gpio_test_dir = self.config['test_gpio_base_dir']
# Build gpio objects for BBB interaction
self.gpio_1 = gpio.GPIO(gpio_pins[0], gpio_test_dir)
self.gpio_2 = gpio.GPIO(gpio_pins[1], gpio_test_dir)
else:
# Build gpio pins
self.gpio_1 = gpio.GPIO(gpio_pins[0])
self.gpio_2 = gpio.GPIO(gpio_pins[1])
# init all pins as output
self.gpio_1.output()
self.gpio_2.output()
def __init__(self):
"""Build IR array abstraction objects."""
# Load config and logger
self.logger = lib.get_logger()
self.config = lib.get_config()
# Number of IR sensors on an array
self.num_ir_units = self.config["irs_per_array"]
# Use accurate reading (ADC)
self.ir_read_adc = self.config["ir_read_adc"]
# Threshold for black/white conversation from analog to binary
self._thresh = self.config["ir_thresh"]
self.reg = self.config["ir_analog_adc_config"]["i2c_registers"]
# NOTE: IR unit select lines are common
ir_analog_input_gpios = self.config["ir_analog_input_gpios"]
# Create IR array objects
#FIXME gpios are not needed for the new adcs
self.arrays = {}
for name, gpio in ir_analog_input_gpios.iteritems():
try:
self.arrays[name] = ir_analog_mod.IRAnalog(name, gpio)
except IOError:
self.logger.error("Unable to create {} IR array".format(name))
self.arrays[name] = None
# Create buffer to store readings from all sensor units
self.reading = {}
for array_name in self.config["ir_analog_adc_config"]["i2c_addr"]:
self.reading[array_name] = [0] * 8
self.last_read_time = None
def __init__(self, gpio_num):
"""Setup logger and GPIO interface.
:param gpio_num: GPIO number used by this solenoid.
:type gpio_num: int
"""
# Get and store logger object
self.logger = lib.get_logger()
# Store ID number of solenoid
self.gpio_num = gpio_num
# Load system configuration
config = lib.get_config()
if config["test_mode"]["solenoid"]:
# Get dir of simulated hardware files from config
test_dir = config["test_gpio_base_dir"]
# Build GPIO object for BBB interaction, provide test dir
self.gpio = gpio_mod.GPIO(self.gpio_num, test_dir)
else:
# Build GPIO object for BBB interaction
self.gpio = gpio_mod.GPIO(self.gpio_num)
# Set GPIO to output signal
self.gpio.output()
# Set solenoid to be initially retracted
self.retract()
self.logger.debug("Setup {}".format(self))
def __init__(self, gpio_pins):
"""Setup GPIO interface."""
# Build logger
self.logger = lib.get_logger()
# Load config
self.config = lib.get_config()
# This variable is inversely proportional to the speed
# of rotation. It is set to max speed by default.
self.time_int = 1.0
if self.config["test_mode"]["stepper"]:
# Get dir of simulated hardware files from config
gpio_test_dir = self.config['test_gpio_base_dir']
# Build gpio objects for BBB interaction, provide test dir
self.gpio_1 = gpio.GPIO(gpio_pins[0], gpio_test_dir)
self.gpio_2 = gpio.GPIO(gpio_pins[1], gpio_test_dir)
self.gpio_3 = gpio.GPIO(gpio_pins[2], gpio_test_dir)
self.gpio_4 = gpio.GPIO(gpio_pins[3], gpio_test_dir)
else:
# Build gpio pins
self.gpio_1 = gpio.GPIO(gpio_pins[0])
self.gpio_2 = gpio.GPIO(gpio_pins[1])
self.gpio_3 = gpio.GPIO(gpio_pins[2])
self.gpio_4 = gpio.GPIO(gpio_pins[3])
self.gpio_1.output()
self.gpio_2.output()
self.gpio_3.output()
self.gpio_4.output()
def __init__(self, cam_config):
self.logger = lib.get_logger()
udev_name = cam_config["udev_name"]
print udev_name
cam_num = find_name(udev_name)
#extract calib data from cam_config
self.a = cam_config["a"]
self.n = cam_config["n"]
self.cam = cv2.VideoCapture(cam_num)
self.cam.set(3, 1280)
self.cam.set(4, 720)
self.resX = int(self.cam.get(3))
self.resY = int(self.cam.get(4))
# QR scanning tools
self.scanner = zbar.ImageScanner()
self.scanner.parse_config('enable')
(self.grabbed, self.frame) = self.cam.read()
self.stopped = True
def __init__(self, pwm_num):
"""Setup logger and PWM interface.
:param pwm_num: PWM number used by this servo.
:type pwm_num: int
"""
# Get and store logger object
self.logger = lib.get_logger()
# Store ID number of servo
self.pwm_num = pwm_num
# Load config
config = lib.get_config()
if config["test_mode"]["servo"]:
# Get dir of simulated hardware files from config
test_dir = config["test_pwm_base_dir"]
# Build PWM object for BBB interaction, provide test dir
self.pwm = pwm_mod.PWM(self.pwm_num, test_dir)
else:
# Build PWM object for BBB interaction
self.pwm = pwm_mod.PWM(self.pwm_num)
# Set servo to middle position
self.pwm.duty = 1500000
self.logger.debug("Setup {}".format(self))
def __init__(self, pwm_num):
"""Setup logger and PWM interface.
:param pwm_num: PWM number used by this servo.
:type pwm_num: int
"""
# Get and store logger object
self.logger = lib.get_logger()
# Store ID number of servo
self.pwm_num = pwm_num
# Load config
config = lib.get_config("bot/config.yaml")
if config["test_mode"]["servo"]:
# Get dir of simulated hardware files from config
test_dir = config["test_pwm_base_dir"]
# Build PWM object for BBB interaction, provide test dir
self.pwm = pwm_mod.PWM(self.pwm_num, test_dir)
else:
# Build PWM object for BBB interaction
self.pwm = pwm_mod.PWM(self.pwm_num)
# Set servo to middle position
self.pwm.duty = 1500000
self.logger.debug("Setup {}".format(self))
def __init__(self, device=default_device):
# * Load system configuration
self.config = lib.get_config()
# * Get and store logger object
self.logger = lib.get_logger()
# * Initialize members
# ** Inpute device (will be set later in __init__)
self.capture = None
self.width = 0
self.height = 0
self.offset = np.float32([0.0, 0.0])
self.auto_exposure = 1.0
# ** Image processing
self.imageIn = None # input image
self.res = None # result/output image (TODO: use separate imageOut)
self.squares = [] # list of squares found
# *** Morphology kernel
self.kernel_rect = cv2.getStructuringElement(cv2.MORPH_RECT, (3, 3))
self.kernel_fat_cross = np.uint8([
[0, 0, 1, 1, 1, 0, 0],
[0, 0, 1, 1, 1, 0, 0],
[1, 1, 1, 1, 1, 1, 1],
[1, 1, 1, 1, 1, 1, 1],
[1, 1, 1, 1, 1, 1, 1],
[0, 0, 1, 1, 1, 0, 0],
[0, 0, 1, 1, 1, 0, 0]]) # custom morph kernel: fat cross
self.kernel_inv_cross = np.uint8([
[1, 1, 1, 0, 1, 1, 1],
[1, 1, 1, 0, 1, 1, 1],
[1, 1, 1, 0, 1, 1, 1],
[0, 0, 0, 0, 0, 0, 0],
[1, 1, 1, 0, 1, 1, 1],
[1, 1, 1, 0, 1, 1, 1],
[1, 1, 1, 0, 1, 1, 1]]) # custom morph kernel: inverted cross
self.kernel = self.kernel_inv_cross # pick kernel that works best
self.logger.debug("Morph. kernel: {}".format(self.kernel))
# ** Misc
self.do_cast_contours = (cv2.__version__ == "2.4.3") # bug workaround
self.refinement_skip_samples = 5 # no. of camera samples to skip
self.refinement_num_samples = 10 # samples to process
self.refinement_good_threshold = 5 # good sample count to accept
# ** Final output
self.location = None # target location
# Open default input device (camera)
if self.open_device(device):
self.logger.info("Opened device: {}".format(device))
else:
self.logger.error("Failed to open device: {}".format(device))
# Set initial camera parameters (NOTE: may crash with recorded video)
self.set_frame_size() # use defaults
self.set_auto_exposure() # use defaults
def __init__(self, name, color_detectors):
"""Setup required pins and get logger/config.
:param name: Identifier for this pot.
:type name: string
:param color_detectors: Pin used by the detection circuit.
:type input_adc_pin: int
"""
self.name = name
self.logger = lib.get_logger()
self.config = lib.get_config()
if self.config["test_mod"]["pot"]:
return
# ADC configuration over I2C
self.pot_config = self.config['pot_config']
self.i2c_addr = self.pot_config['i2c_addr'][name]
self.color_gpio = self.config["simon"]["colors"]
self.color_detectors = color_detectors
if i2c_available:
# Open I2C bus
self.bus = smbus.SMBus(self.pot_config['i2c_bus'])
# Configure ADC using I2C commands
for reg_name, reg in self.pot_config['i2c_registers'].iteritems():
self.set_pot_byte(reg['cmd'], reg['init'])
self.logger.debug("Setup {} (on I2C addr: {})".format(
self, hex(self.i2c_addr)))
def setUp(self):
"""Setup test hardware files and build motor object."""
config = path.dirname(path.realpath(__file__))+"/test_config.yaml"
self.config = lib.get_config(config)
self.logger = lib.get_logger()
self.logger.info("Running {}()".format(self._testMethodName))
motor_conf = self.config['dmcc_drive_motors']
self.motor_set = DMCCMotorSet(motor_conf)
def setUp(self):
"""Setup test hardware files and build motor object."""
config = path.dirname(path.realpath(__file__)) + "/test_config.yaml"
self.config = lib.get_config(config)
self.logger = lib.get_logger()
self.logger.info("Running {}()".format(self._testMethodName))
motor_conf = self.config['dmcc_drive_motors']
self.motor_set = DMCCMotorSet(motor_conf)
def __init__(self, systems):
"""Override Thread.__init__, build ZMQ PUB socket.
:param systems: Objects that own resources and drive functionally.
:type systems: dict
"""
# Call superclass __init__ methods
threading.Thread.__init__(self)
# Load configuration and logger
self.config = lib.get_config()
self.logger = lib.get_logger()
# Unpack required objects from systems
self.driver = systems["driver"]
# Build ZMQ publisher socket
self.context = zmq.Context()
self.pub_sock = self.context.socket(zmq.PUB)
self.pub_addr = "{protocol}://{host}:{port}".format(
protocol=self.config["server_protocol"],
host=self.config["server_bind_host"],
port=self.config["pub_server_port"])
self.pub_sock.bind(self.pub_addr)
# Mapping of published topics to methods that give their value
# I know some of these lines are too long. F**k it, it's clean code.
self.topics = {
"drive_motor_detail_br": self.driver.motors["back_right"].__str__,
"drive_motor_detail_fr": self.driver.motors["front_right"].__str__,
"drive_motor_detail_bl": self.driver.motors["back_left"].__str__,
"drive_motor_detail_fl": self.driver.motors["front_left"].__str__,
"drive_motor_vel_br":
self.driver.motors["back_right"].get_velocity,
"drive_motor_vel_fr":
self.driver.motors["front_right"].get_velocity,
"drive_motor_vel_bl":
self.driver.motors["back_left"].get_velocity,
"drive_motor_vel_fl":
self.driver.motors["front_left"].get_velocity,
"drive_motor_speed_br":
self.driver.motors["back_right"].get_speed,
"drive_motor_speed_fr":
self.driver.motors["front_right"].get_speed,
"drive_motor_speed_bl":
self.driver.motors["back_left"].get_speed,
"drive_motor_speed_fl":
self.driver.motors["front_left"].get_speed,
"drive_motor_dir_br":
self.driver.motors["back_right"].get_direction,
"drive_motor_dir_fr":
self.driver.motors["front_right"].get_direction,
"drive_motor_dir_bl":
self.driver.motors["back_left"].get_direction,
"drive_motor_dir_fl":
self.driver.motors["front_left"].get_direction,
}
def setUp(self):
"""Get config, set simulation pins to known state, set test flag."""
# Load config and logger
self.config = lib.get_config("bot/config.yaml")
self.logger = lib.get_logger()
# Set testing flag in config
self.orig_test_state = self.config["testing"]
lib.set_testing(True)
def setUp(self):
"""Get config, set simulation pins to known state, set test flag."""
# Load config and logger
self.config = lib.get_config("bot/config.yaml")
self.logger = lib.get_logger()
# Set testing flag in config
self.orig_test_state = self.config["testing"]
lib.set_testing(True)
def __init__(self, pwm_num, gpio_num=None, inverted=False):
"""Build GPIO and PWM pins, set initial values.
Note that the default gpio_num=None param implies that the motor
has no direction. Its direction should be manually changed by
swapping the wires that drive it.
:param pwm_num: PWM number for this motor.
:type pwm_num: int
:param gpio_num: Optional GPIO number for this motor.
:type gpio_num: int
:param inverted: Whether to treat direction as inverted.
:type inverted: bool
"""
# Get and store logger object
self.logger = lib.get_logger()
# Store PWM and GPIO numbers of motor
self.pwm_num = pwm_num
self.gpio_num = gpio_num
# Set motor-specific forward and reverse values based on inverted
# TODO: Make this a config flag
self.invert(inverted)
# Load system configuration
config = lib.get_config("bot/config.yaml")
if config["test_mode"]["motor"]:
# Get dir of simulated hardware files from config
pwm_test_dir = config["test_pwm_base_dir"]
# Build PWM object for BBB interaction, provide test dir
self.pwm = pwm_mod.PWM(self.pwm_num, pwm_test_dir)
if self.gpio_num is not None:
# Build GPIO object for BBB interaction, provide test dir
gpio_test_dir = config["test_gpio_base_dir"]
self.gpio = gpio_mod.GPIO(self.gpio_num, gpio_test_dir)
else:
# Build PWM object for BBB interaction
self.pwm = pwm_mod.PWM(self.pwm_num)
if self.gpio_num is not None:
# Build GPIO object for BBB interaction
self.gpio = gpio_mod.GPIO(self.gpio_num)
# Polarity should be 0 to get X% high at X PWM.
self.pwm.polarity = 0
# Setup initial speed and direction
self.speed = 0
if self.gpio_num is not None:
self.direction = FORWARD
self.logger.debug("Setup {}".format(self))
def __init__(self, systems):
"""Override Thread.__init__, build ZMQ PUB socket.
:param systems: Objects that own resources and drive functionally.
:type systems: dict
"""
# Call superclass __init__ methods
threading.Thread.__init__(self)
# Load configuration and logger
self.config = lib.get_config()
self.logger = lib.get_logger()
# Unpack required objects from systems
self.driver = systems["driver"]
# Build ZMQ publisher socket
self.context = zmq.Context()
self.pub_sock = self.context.socket(zmq.PUB)
self.pub_addr = "{protocol}://{host}:{port}".format(
protocol=self.config["server_protocol"],
host=self.config["server_bind_host"],
port=self.config["pub_server_port"])
self.pub_sock.bind(self.pub_addr)
# Mapping of published topics to methods that give their value
# I know some of these lines are too long. F**k it, it's clean code.
self.topics = {
"drive_motor_detail_br": self.driver.motors["back_right"].__str__,
"drive_motor_detail_fr": self.driver.motors["front_right"].__str__,
"drive_motor_detail_bl": self.driver.motors["back_left"].__str__,
"drive_motor_detail_fl": self.driver.motors["front_left"].__str__,
"drive_motor_vel_br":
self.driver.motors["back_right"].get_velocity,
"drive_motor_vel_fr":
self.driver.motors["front_right"].get_velocity,
"drive_motor_vel_bl": self.driver.motors["back_left"].get_velocity,
"drive_motor_vel_fl":
self.driver.motors["front_left"].get_velocity,
"drive_motor_speed_br": self.driver.motors["back_right"].get_speed,
"drive_motor_speed_fr":
self.driver.motors["front_right"].get_speed,
"drive_motor_speed_bl": self.driver.motors["back_left"].get_speed,
"drive_motor_speed_fl": self.driver.motors["front_left"].get_speed,
"drive_motor_dir_br":
self.driver.motors["back_right"].get_direction,
"drive_motor_dir_fr":
self.driver.motors["front_right"].get_direction,
"drive_motor_dir_bl":
self.driver.motors["back_left"].get_direction,
"drive_motor_dir_fl":
self.driver.motors["front_left"].get_direction,
}
def __init__(self):
"""Setup and store logger and configuration."""
# Load and store logger
self.logger = lib.get_logger()
# Load and store configuration dict
self.config = lib.get_config()
self.gun = wheel_gun.WheelGun()
self.turret = turret.Turret()
self.ultrasonics = ultrasonic.Ultrasonic()
def __init__(self):
self.logger = lib.get_logger()
self.config = lib.get_config()
if self.config["test_mode"]["etch_a_sketch"]:
# add code for test mode
pass
else:
self.vert_motor = Stepper_motor(self.config["etch_a_sketch_motors"]["left_stepper"])
self.horiz_motor = Stepper_motor(self.config["etch_a_sketch_motors"]["right_stepper"])
def __init__(self):
"""Load logger and build pitch and yaw servo abstractions."""
# Load and store logger object
self.logger = lib.get_logger()
# Load config
config = lib.get_config()
self.config = config['turret']
# Build and store abstraction of servos for x and y axis movement
self.servos = {}
for servo, conf in self.config['servos'].items():
self.servos[servo] = s_mod.Servo(conf['PWM'])
def __init__(self, i2c_bus, i2c_address):
"""Initialized I2C device"""
self.logger = lib.get_logger()
self.bot_config = lib.get_config()
# Bus location and address of ADC.
self.bus = i2c_bus
self.addr = i2c_address
# Handle off-bone runs
if self.bot_config["test_mode"]["ADC"]:
self.logger.debug("Running in test mode")
else:
self.logger.debug("Running in non-test mode")
def __init__(self, motor_config):
"""Initialize a set of DMCCs and their associated motors
:param motor_config: Config entry mapping motor names to DMCC ids and
motor indices
Dictionary entries are in the format:
<motor_name>: { board_num: [0-3], motor_num: [1-2] }
"""
self.config = lib.get_config()
self.logger = lib.get_logger()
self.is_testing = self.config["test_mode"]["DMCC"]
# print "Testing: ", self.config["testing"]
# print pyDMCC.lib._config
# This instantiates all DMCCs in every DMCCManager, which is probably
# not optimal, which works fine for our purposes. Potentially better
# approaches:
# - global state: shared dmccs dictionary, instantiated once
# - selected instantiation: only initialize the dmccs we are control
if not self.is_testing:
dmccs = pyDMCC.autodetect()
self.logger.debug("Found %d physical DMCC boards" % len(dmccs))
else:
self.logger.debug("Skipping autodetect due to test mode")
dmccs = defaultdict(
lambda: pyDMCC.DMCC(
0, verify=False, bus=None, logger=self.logger))
self.logger.debug("DMCC Motor conf: {}".format(dmccs))
self.motors = {}
for name, conf in motor_config.items():
if 'invert' in conf.keys():
invert = conf['invert']
else:
invert = False
try:
self.motors[name] = DMCCMotor(
dmccs[conf['board_num']], conf['motor_num'], invert)
except KeyError:
self.logger.error(
"Bad motor definition for motor: '{}'".format(
name))
raise
self.logger.debug("Setup {}".format(self))
def __init__(self, i2c_bus, i2c_address):
"""Initialized I2C device"""
self.logger = lib.get_logger()
self.bot_config = lib.get_config()
# Bus location and address of ADC.
self.bus = i2c_bus
self.addr = i2c_address
# Handle off-bone runs
if self.bot_config["test_mode"]["ADC"]:
self.logger.debug("Running in test mode")
else:
self.logger.debug("Running in non-test mode")
def __init__(self, cape_config):
"""Initialize vars"""
self.logger = lib.get_logger()
self.bot_config = lib.get_config()
self.bus = smbus.SMBus(cape_config["i2c_bus"])
self.addr = cape_config["i2c_addr"]
#TODO(Ahmed): Figure out how to use regs
self.reg = 0xFF
if self.bot_config["test_mode"]["servo_cape"]:
self.logger.debug("running in test mode")
else:
self.logger.debug("non test-mode, real hardware")
def __init__(self, cape_config):
"""Initialize vars"""
self.logger = lib.get_logger()
self.bot_config = lib.get_config()
self.bus = smbus.SMBus(cape_config["i2c_bus"])
self.addr = cape_config["i2c_addr"]
#TODO(Ahmed): Figure out how to use regs
self.reg = 0xFF
if self.bot_config["test_mode"]["servo_cape"]:
self.logger.debug("running in test mode")
else:
self.logger.debug("non test-mode, real hardware")
def __init__(self, testing=None, config_file="bot/config.yaml"):
"""Build ZMQ REP socket and instantiate bot systems.
:param testing: True if running on simulated HW, False if on bot.
:type testing: boolean
:param config_file: Name of file to read configuration from.
:type config_file: string
"""
# Register signal handler, shut down cleanly (think motors)
signal.signal(signal.SIGINT, self.signal_handler)
# Load configuration and logger
self.config = lib.get_config(config_file)
self.logger = lib.get_logger()
# Testing flag will cause objects to run on simulated hardware
if testing is True or testing == "True":
self.logger.info("CtrlServer running in test mode")
lib.set_testing(True)
elif testing is None:
self.logger.info(
"Defaulting to config testing flag: {}".format(
self.config["testing"]))
lib.set_testing(self.config["testing"])
else:
self.logger.info("CtrlServer running in non-test mode")
lib.set_testing(False)
# Build socket to listen for requests
self.context = zmq.Context()
self.ctrl_sock = self.context.socket(zmq.REP)
self.server_bind_addr = "{protocol}://{host}:{port}".format(
protocol=self.config["server_protocol"],
host=self.config["server_bind_host"],
port=self.config["ctrl_server_port"])
try:
self.ctrl_sock.bind(self.server_bind_addr)
except zmq.ZMQError:
self.logger.error("ZMQ error. Is a server already running?")
self.logger.warning("May be connected to an old server instance.")
sys.exit(1)
self.systems = self.assign_subsystems()
self.logger.info("Control server initialized")
# Don't spawn pub_server until told to
self.pub_server = None
def setUp(self):
"""Get config, set simulation pins to known state, set test flag."""
# Load config and logger
self.config = lib.get_config("bot/config.yaml")
self.logger = lib.get_logger()
# Set testing flag in config
self.orig_test_state = self.config["testing"]
lib.set_testing(True)
# Write known values to all simulated hardware files
self.setup_turret_servos()
self.setup_laser()
self.setup_gun_trigger()
self.setup_ir_select_gpios()
self.setup_ir_analog_input_gpios()
def __init__(self, motor_config):
"""Initialize a set of DMCCs and their associated motors
:param motor_config: Config entry mapping motor names to DMCC ids and
motor indices
Dictionary entries are in the format:
<motor_name>: { board_num: [0-3], motor_num: [1-2] }
"""
self.config = lib.get_config("bot/config.yaml")
self.logger = lib.get_logger()
self.is_testing = self.config["test_mode"]["DMCC"]
# print "Testing: ", self.config["testing"]
# print pyDMCC.lib._config
# This instantiates all DMCCs in every DMCCManager, which is probably
# not optimal, which works fine for our purposes. Potentially better
# approaches:
# - global state: shared dmccs dictionary, instantiated once
# - selected instantiation: only initialize the dmccs we are control
if not self.is_testing:
dmccs = pyDMCC.autodetect()
self.logger.debug("Found %d physical DMCC boards" % len(dmccs))
else:
self.logger.debug("Skipping autodetect due to test mode")
dmccs = defaultdict(lambda: pyDMCC.DMCC(
0, verify=False, bus=None, logger=self.logger))
self.logger.debug("DMCC Motor conf: {}".format(dmccs))
self.motors = {}
for name, conf in motor_config.items():
if 'invert' in conf.keys():
invert = conf['invert']
else:
invert = False
try:
self.motors[name] = DMCCMotor(dmccs[conf['board_num']],
conf['motor_num'], invert)
except KeyError:
self.logger.error(
"Bad motor definition for motor: '{}'".format(name))
raise
self.logger.debug("Setup {}".format(self))
def __init__(self, testing=None, config_file="bot/config.yaml"):
"""Build ZMQ REP socket and instantiate bot systems.
:param testing: True if running on simulated HW, False if on bot.
:type testing: boolean
:param config_file: Name of file to read configuration from.
:type config_file: string
"""
# Register signal handler, shut down cleanly (think motors)
signal.signal(signal.SIGINT, self.signal_handler)
# Load configuration and logger
self.config = lib.get_config(config_file)
self.logger = lib.get_logger()
# Testing flag will cause objects to run on simulated hardware
if testing is True or testing == "True":
self.logger.info("CtrlServer running in test mode")
lib.set_testing(True)
elif testing is None:
self.logger.info("Defaulting to config testing flag: {}".format(
self.config["testing"]))
lib.set_testing(self.config["testing"])
else:
self.logger.info("CtrlServer running in non-test mode")
lib.set_testing(False)
# Build socket to listen for requests
self.context = zmq.Context()
self.ctrl_sock = self.context.socket(zmq.REP)
self.server_bind_addr = "{protocol}://{host}:{port}".format(
protocol=self.config["server_protocol"],
host=self.config["server_bind_host"],
port=self.config["ctrl_server_port"])
try:
self.ctrl_sock.bind(self.server_bind_addr)
except zmq.ZMQError:
self.logger.error("ZMQ error. Is a server already running?")
self.logger.warning("May be connected to an old server instance.")
sys.exit(1)
self.systems = self.assign_subsystems()
self.logger.info("Control server initialized")
# Don't spawn pub_server until told to
self.pub_server = None
def __init__(self, name, params):
"""Setup required pins and get logger.
NOTE: The required pins are not known, so this is a stub.
:param name: Indicates position of the US on the bot (front, back...).
:type name: string
:param params: Parameters used to initialize this ultrasonic sensor.
:type params: dict
"""
# Get config and logger
self.config = lib.get_config()
self.logger = lib.get_logger()
self.is_testing = self.config["test_mode"]["ultrasonics"]
# Store name and params, initialize other members
self.name = name
self.gpio_num = params["gpio"]
self.location = np.float32(params["location"])
self.direction = np.float32(params["direction"])
self.gpio = None
self._distance = 0.0
# Testing setup
if self.is_testing:
# Get dir of simulated hardware files from config
gpio_test_dir = self.config["test_gpio_base_dir"]
# Build GPIO object in test mode
self.gpio = gpio_mod.GPIO(self.gpio_num, gpio_test_dir)
else:
try:
self.gpio = gpio_mod.GPIO(self.gpio_num)
except Exception as e:
self.logger.error(
"GPIOs could not be initialized. "
+ "Not on the bone? Run unit test instead. "
+ "Exception: {}".format(e)
)
self.logger.debug("Setup {}".format(self))
# Warn user that this code is only a stub
self.logger.warning("US abstraction not implemented, range will be 0.")
def __init__(self):
"""Build logger, get config, build motor, laser and trigger pins."""
# Load and store logger
self.logger = lib.get_logger()
# Load and store configuration dict
self.config = lib.get_config()
self.max_trigger_duration = float(
self.config['gun']['max_trigger_duration']) # 0.25 secs.
# 0.99895" (25.373mm) diameter
self.wheel_radius = 0.012687 # meters
self.ticks_per_rev = 48 # CPR on motor shaft per Pololu spec, verified
self.dmcc_to_tps = 20.4 # dmcc reported velocity must be scaled
self._dart_velocity = None # shadow velocity for testing
self._wheel_velocity = None # shadow velocity for testing
motor_config = self.config['gun']['dmcc_wheel_motors']
self.wheel_motors = DMCCMotorSet(motor_config)
# Build GPIOs to control laser, motors and triggers
self.trigger_gpios = dict()
self.laser_gpio = None
if self.config["test_mode"]["wheelgun"]:
# Get dir of simulated hardware files from config
gpio_test_dir = self.config['test_gpio_base_dir']
# Build GPIO pins in test mode
# NOTE: Simulated directories and files must already exist
# The unittest superclass will build them.
self.laser_gpio = gpio.GPIO(
self.config['gun']['laser_gpio'], gpio_test_dir)
self.trigger_gpios['retract'] = gpio.GPIO(
self.config['gun']['trigger_gpios']['retract'], gpio_test_dir)
self.trigger_gpios['advance'] = gpio.GPIO(
self.config['gun']['trigger_gpios']['advance'], gpio_test_dir)
else:
# Build GPIO pins in live (normal) mode
self.laser_gpio = gpio.GPIO(
self.config['gun']['laser_gpio']) # gpio8
self.trigger_gpios['retract'] = gpio.GPIO(
self.config['gun']['trigger_gpios']['retract']) # gpio74
self.trigger_gpios['advance'] = gpio.GPIO(
self.config['gun']['trigger_gpios']['advance']) # gpio72
def __init__(self):
# Build logger
self.logger = lib.get_logger()
# Build subsystems
self.ir_hub = ir_hub_mod.IRHub()
self.ir_hub.thrsh = 100
self.driver = mec_driver_mod.MecDriver()
self.color_sensor = color_sensor_mod.ColorSensor()
# Build PIDs
# FIXME 1 no longer in use
self.front_right = pid_mod.PID();
self.front_right_error = 0.0
self.front_left = pid_mod.PID();
self.front_left_error = 0.0
self.back_right = pid_mod.PID();
self.back_right_error = 0.0
self.back_left = pid_mod.PID();
self.back_left_error = 0.0
self.strafe = pid_mod.PID()
self.strafe_error = 0.0
self.rotate_pid = pid_mod.PID()
self.rotate_error = 0.0
self.error = "NONE"
# motor variables
# FIXME 2 same as before
self.translate_speed = 75
self.prev_rate = 0
# state variables
self.heading = None # must be initialize by caller
self.front_state = Follower.No_Line
self.back_state = Follower.No_Line
self.left_state = Follower.No_Line
self.right_state = Follower.No_Line
# post error vars
self.intersection = False
self.lost_line = False
self.timeLastUpdated = -1.0
self.on_x = False
def __init__(self):
self.logger = lib.get_logger()
self.bot_config = lib.get_config()
# Handle off-bone runs
self.testing = self.bot_config["test_mode"]["slider_switch"]
if not self.testing:
self.logger.info("Running in non-test mode")
# Setup ready signal GPIO:
self.left_gpio = self.bot_config["slider_switch"]["left_gpio"]
self.right_gpio = self.bot_config["slider_switch"]["right_gpio"]
self.left_course_gpio = gpio_mod.GPIO(self.left_gpio)
self.right_course_gpio = gpio_mod.GPIO(self.right_gpio)
self.left_course_gpio.input()
self.right_course_gpio.input()
else:
self.logger.info("Running in test mode")
def __init__(self):
# Build logger
self.logger = lib.get_logger()
# Build subsystems
self.ir_hub = ir_hub_mod.IRHub()
self.ir_hub.thrsh = 100
self.driver = mec_driver_mod.MecDriver()
self.color_sensor = color_sensor_mod.ColorSensor()
# Build PIDs
# FIXME 1 no longer in use
self.front_right = pid_mod.PID()
self.front_right_error = 0.0
self.front_left = pid_mod.PID()
self.front_left_error = 0.0
self.back_right = pid_mod.PID()
self.back_right_error = 0.0
self.back_left = pid_mod.PID()
self.back_left_error = 0.0
self.strafe = pid_mod.PID()
self.strafe_error = 0.0
self.rotate_pid = pid_mod.PID()
self.rotate_error = 0.0
self.error = "NONE"
# motor variables
# FIXME 2 same as before
self.translate_speed = 75
self.prev_rate = 0
# state variables
self.heading = None # must be initialize by caller
self.front_state = Follower.No_Line
self.back_state = Follower.No_Line
self.left_state = Follower.No_Line
self.right_state = Follower.No_Line
# post error vars
self.intersection = False
self.lost_line = False
self.timeLastUpdated = -1.0
self.on_x = False
def __init__(self):
self.logger = lib.get_logger()
self.bot_config = lib.get_config()
# Handle off-bone runs
self.testing = self.bot_config["test_mode"]["slider_switch"]
if not self.testing:
self.logger.info("Running in non-test mode")
# Setup ready signal GPIO:
self.left_gpio = self.bot_config["slider_switch"]["left_gpio"]
self.right_gpio = self.bot_config["slider_switch"]["right_gpio"]
self.left_course_gpio = gpio_mod.GPIO(self.left_gpio)
self.right_course_gpio = gpio_mod.GPIO(self.right_gpio)
self.left_course_gpio.input()
self.right_course_gpio.input()
else:
self.logger.info("Running in test mode")
def __init__(self, rail_cars=0):
self.config = lib.get_config()
self.PID_values = self.config["IR_PID"]
self.device = IR() # INSTANTIATE ONLY ONCE
self.north = Side("North Left", "North Right", self.device.read_values, self.PID_values["North"]["diff"], self.PID_values["North"]["dist"])
self.south = Side("South Left", "South Right", self.device.read_values, self.PID_values["South"]["diff"], self.PID_values["South"]["dist"])
self.east = Side("East Top", "East Bottom", self.device.read_values, self.PID_values["East"]["diff"], self.PID_values["East"]["dist"])
self.west = Side("West Top", "West Bottom", self.device.read_values, self.PID_values["West"]["diff"], self.PID_values["West"]["dist"])
self.driver = OmniDriver()
self.sides = {"north": self.north,
"south": self.south,
"west": self.west,
"east": self.east}
self.moving = False
self.logger = lib.get_logger()
mapping = ["EXIT", "west", "east", "EXIT"]
self.rail_cars_side = mapping[rail_cars]
def __init(self):
"""Sets up the light detecting hardware.
"""
self.config = lib.get_config()
self.logger = lib.get_logger()
self.color_gpio = self.config["simon"]["colors"]
self.color_detectors = dict()
for color in self.color_gpio:
self.color_detectors[color] = \
bbb.mod.GPIO(self.color_gpio[color])
self.pot = Pot("pot1", self.color_detectors)
# init all the detectors as inputs
for d in self.color_detectors:
self.color_detectors[d].input()
def __init__(self, dmcc, motor_num, invert=False):
"""Wraps an individual pyDMCC motor
:param dmcc: Controlling pyDMCC.DMCC object, None when testing
:param motor_num: Motor number in motor_num_range.
:param invert: Set to True if all values should be inverted
:type motor_num: int
Attribute real_motor is the instantiated pyDMCC.Motor
"""
self.config = lib.get_config()
self.logger = lib.get_logger()
self.is_testing = self.config["test_mode"]["DMCC"]
self.dmcc = dmcc
self.real_motor = dmcc.motors[motor_num]
self.motor_num = motor_num
self.invert = invert
if invert:
self.inversion_multiplier = -1
else:
self.inversion_multiplier = 1
if self.is_testing:
self._pos_kP = self._pos_kI = self._pos_kD = 0
self._vel_kP = self._vel_kI = self._vel_kD = 0
else:
self.logger.critical("Testing : {}".format(self.is_testing))
self._pos_kP, self._pos_kI, self._pos_kD \
= self.real_motor.position_pid
self._vel_kP, self._vel_kI, self._vel_kD \
= self.real_motor.velocity_pid
self._power = 0 # last set power; DMCC can't read back power (yet!)
if self.is_testing:
self._position = 0 # last set position, only when testing
self._velocity = 0 # last set velocity, only when testing
self.logger.debug("Setup {}".format(self))
def __init__(self, dmcc, motor_num, invert=False):
"""Wraps an individual pyDMCC motor
:param dmcc: Controlling pyDMCC.DMCC object, None when testing
:param motor_num: Motor number in motor_num_range.
:param invert: Set to True if all values should be inverted
:type motor_num: int
Attribute real_motor is the instantiated pyDMCC.Motor
"""
self.config = lib.get_config()
self.logger = lib.get_logger()
self.is_testing = self.config["test_mode"]["DMCC"]
self.dmcc = dmcc
self.real_motor = dmcc.motors[motor_num]
self.motor_num = motor_num
self.invert = invert
if invert:
self.inversion_multiplier = -1
else:
self.inversion_multiplier = 1
if self.is_testing:
self._pos_kP = self._pos_kI = self._pos_kD = 0
self._vel_kP = self._vel_kI = self._vel_kD = 0
else:
self.logger.critical("Testing : {}".format(self.is_testing))
self._pos_kP, self._pos_kI, self._pos_kD \
= self.real_motor.position_pid
self._vel_kP, self._vel_kI, self._vel_kD \
= self.real_motor.velocity_pid
self._power = 0 # last set power; DMCC can't read back power (yet!)
if self.is_testing:
self._position = 0 # last set position, only when testing
self._velocity = 0 # last set velocity, only when testing
self.logger.debug("Setup {}".format(self))
def __init__(self):
"""Initialized I2C device, LED brightness PWM pin."""
self.logger = lib.get_logger()
self.bot_config = lib.get_config()
# Setup ready signal GPIO:
self.gpio_num = self.bot_config["color_sensor"]["ready_signal"]
self.ready_gpio = gpio_mod.GPIO(self.gpio_num)
self.ready_gpio.input()
# Handle off-bone runs
self.testing = self.bot_config["test_mode"]["color_sensor"]
if not self.testing:
self.logger.debug("Running in non-test mode")
# Setup I2C
I2CDevice.__init__(self, 1, 0x29, config='tcs3472_i2c.yaml')
enable = self.registers['ENABLE']
id = self.registers['ID']
status = self.registers['STATUS']
enable.write('PON', 'Enable')
enable.write('AEN', 'Enable')
enable = self.registers['ENABLE'] # TODO: Is this needed?
# Setup PWM pin for dimming LED
self.pwm_num = self.bot_config["color_sensor"]["LED_PWM"]
self.pwm = pwm_mod.PWM(self.pwm_num)
# Duty cycle = 50% (from 20msec)
self.pwm.duty = 1000000
else:
self.logger.debug("Running in test mode")
# Fake device at address "0x00"
I2CDevice.__init__(self, 1, 0x00, config='tcs3472_i2c.yaml')
# Gets base values for comparisons to future readings.
self.get_baseline()
def __init__(self):
# Load and store logger
self.logger = lib.get_logger()
# Load and store configuration dict
self.config = lib.get_config()
if self.config["test_mode"]["simon_player"]:
# yet to add trivial code
pass
else:
# Build the 5 micromotors
self.motor1 = Micromotor(self.config["simon"]["motor1"])
self.motor2 = Micromotor(self.config["simon"]["motor2"])
self.motor3 = Micromotor(self.config["simon"]["motor3"])
self.motor4 = Micromotor(self.config["simon"]["motor4"])
self.motor5 = Micromotor(self.config["simon"]["motor5"])
# TODO (Vijay) : Build the input detectors
self.input_detector = LightDetector()
def __init__(self):
"""Initialized I2C device, LED brightness PWM pin."""
self.logger = lib.get_logger()
self.bot_config = lib.get_config()
# Handle off-bone runs
self.testing = self.bot_config["test_mode"]["color_sensor"]
if not self.testing:
self.logger.debug("Running in non-test mode")
# Setup I2C
I2CDevice.__init__(self, 1, 0x29, config='tcs3472_i2c.yaml')
enable = self.registers['ENABLE']
# id = self.registers['ID']
# status = self.registers['STATUS']
enable.write('PON', 'Enable')
enable.write('AEN', 'Enable')
enable = self.registers['ENABLE'] # TODO: Is this needed?
# Setup PWM pin for dimming LED
self.pwm_num = self.bot_config["color_sensor"]["LED_PWM"]
self.pwm = pwm_mod.PWM(self.pwm_num)
# Duty cycle = 50% (from 20msec)
self.pwm.duty = 1000000
# Setup ready signal GPIO:
self.gpio_num = self.bot_config["color_sensor"]["ready_signal"]
self.ready_gpio = gpio_mod.GPIO(self.gpio_num)
self.ready_gpio.input()
else:
self.logger.debug("Running in test mode")
# Fake device at address "0x00"
I2CDevice.__init__(self, 1, 0x00, config='tcs3472_i2c.yaml')
# Gets base values for comparisons to future readings.
self.get_baseline()
def test_type(self):
"""Test that object returned is of proper type."""
assert type(lib.get_logger()) is logging.Logger
"""Test case for pid"""
import bot.lib.lib as lib
import tests.test_bot as test_bot
import bot.follower.pid as pid_mod
# Build logger
logger = lib.get_logger()
class TestPID(test_bot.TestBot):
"""Test PID."""
def setUp(self):
"""Get config and built IR object."""
super(TestPID, self).setUp()
# Build PID object
self.pid = pid_mod.PID()
def tearDown(self):
"""Restore testing flag state in config file."""
super(TestPID, self).tearDown()
def test_pid_kp(self):
"""Test the funtionaty of pid"""
self.pid.set_k_values(1, 0, 0)
test_inputs = [0, 0, 0, 1, 2, 3, 4, 5, 6, 0, 0, 1, 2, 3, 4, 5, 6, 7]
print "Run pid kp test"
for x in test_inputs:
test_output = self.pid.pid(0, x, 1)
assert test_output == -x, "{} != {}".format(test_output, x)
def setUp(self):
config = path.dirname(path.realpath(__file__)) + "/test_config.yaml"
self.config = lib.get_config(config)
self.logger = lib.get_logger()
self.logger.info("Running {}()".format(self._testMethodName))
def __init__(self):
"""Setup and store logger and config."""
self.logger = lib.get_logger()
self.config = lib.get_config()
