def __init__(self):
print("Initializing QBot2e...")
# Define DAQ type
self._card = HIL()
self._card.open("qbot2e", "0")
# Configure analog channels
self._ai_channels = array('I', [0, 4, 5])
self._ai_buffer = array('d', [0.0] * len(self._ai_channels))
# Configure digital channels
self._do_channels = array('I', [i for i in range(28, 36)])
self._do_buffer = array('b', [0, 0, 0, 0, 1, 1, 1, 1])
self._di_channels = array('I', [i for i in range(28, 59)])
self._di_buffer = array('b', [0] * len(self._di_channels))
#self._card.set_digital_directions(self._di_channels, self._di_num_channels, self._do_channels, self._do_num_channels)
# Configure other channels
self._oo_channels = array('I', [2000, 2001])
self._oo_buffer = array('d', [0.0] * len(self._oo_channels))
self._oi_channels = array(
'I', [1002, 3000, 3001, 3002, 11000, 11001, 12000, 16000])
self._oi_buffer = array('d', [0.0] * len(self._oi_channels))
# Configure encoder channels
self._enc_channels = array('I', [0, 1])
self._enc_buffer = array('i', [0] * len(self._enc_channels))
# Reset QBot2e
self.reset()
print("QBot2e Initialized")
def __init__(self):
# Define DAQ type as Q2-USB
self._card = HIL()
self._card.open("q2_usb", "0")
# Set Q2-USB speed to noraml
self._card.set_card_specific_options("update_rate=normal;", 50)
# Configure analog input channels
self._ai_channels = array('I', [0, 1])
self._ai_buffer = array('d', [0.0] * len(self._ai_channels))
# Configure analog output channels
self._ao_channels = array('I', [0, 1])
self._ao_buffer = array('d', [0.0] * len(self._ao_channels))
# Configure encoder input channels
self._enc_channels = array('I', [0, 1])
self._enc_buffer = array('i', [0] * len(self._enc_channels))
# Write 0 V to all AO channels
self._card.write_analog(self._ao_channels, len(self._ao_channels),
self._ao_buffer)
# Set encoder counts to 0
_init_enc_counts = array('i', [0, 0])
self._card.set_encoder_counts(self._enc_channels,
len(self._enc_channels),
_init_enc_counts)
print("Q2-USB DAQ Initialized")
def __init__(self):
''' This function configures the QCar and returns a handle to the QCar card. Use the handle for other methods such as qcar_io or terminate_qcar.'''
self.card = HIL()
try:
# Open the Card
self.card.open("qcar", "0")
if self.card.is_valid():
# Set PWM mode (duty cycle) and frequency
self.card.set_pwm_mode(
np.array([0], dtype=np.uint32),
len(np.array([0], dtype=np.uint32)),
np.array([PWMMode.DUTY_CYCLE], dtype=np.int32))
self.card.set_pwm_frequency(
np.array([0], dtype=np.uint32),
len(np.array([0], dtype=np.uint32)),
np.array([60e6 / 4096], dtype=np.float64))
# Set Motor coast to 0
self.card.write_digital(
np.array([40], dtype=np.uint32),
len(np.array([40], dtype=np.uint32)),
np.zeros(len(np.array([0], dtype=np.uint32)),
dtype=np.float64))
# Set Encoder Quadrature
encoder_channels = np.array([0], dtype=np.uint32)
num_encoder_channels = len(encoder_channels)
self.card.set_encoder_quadrature_mode(
encoder_channels, num_encoder_channels,
np.array([4], dtype=np.uint32))
self.card.set_encoder_filter_frequency(
encoder_channels, num_encoder_channels,
np.array([60e6 / 1], dtype=np.uint32))
self.card.set_encoder_counts(encoder_channels,
num_encoder_channels,
np.zeros(1, dtype=np.int32))
print('QCar configured successfully.')
except HILError as h:
print(h.get_error_message())
class QCar():
#region: Buffers
# Throttle Write Only - PWM channel 0 is mtr cmd
write_pwm_channel_throttle = np.array([0], dtype=np.int32)
write_pwm_buffer_throttle = np.array([0], dtype=np.float64)
# Steering Write Only - Other channel 1000 is steering cmd
write_other_channel_steering = np.array([1000], dtype=np.int32)
write_other_buffer_steering = np.array([0], dtype=np.float64)
# LEDs Write Only - Other channel 11000:11003 + 11008:11011 are LEDs
write_other_channels_LEDs = np.array(
[11008, 11009, 11010, 11011, 11000, 11001, 11002, 11003],
dtype=np.int32)
write_other_buffer_LEDs = np.zeros(8, dtype=np.float64)
# User LEDs Write Only - Other channel 11004:11007 are User LEDs
write_other_channels_usr_LEDs = np.array([11004, 11005, 11006, 11007],
dtype=np.int32)
write_other_buffer_usr_LEDs = np.zeros(4, dtype=np.float64)
# Steering and LEDs Write - Other channel 1000 is steering cmd and 11000:11003 + 11008:11011 are LEDs
write_other_channels_str_LEDs = np.array(
[1000, 11008, 11009, 11010, 11011, 11000, 11001, 11002, 11003],
dtype=np.int32)
write_other_buffer_str_LEDs = np.append(np.array([0], dtype=np.float64),
np.zeros(8, dtype=np.float64))
# Initialize return arrays
mtr_current, bat_voltage = np.zeros(2, dtype=np.float64)
mtr_encoder = np.zeros(1, dtype=np.int32)
accelerometer = np.zeros(3, dtype=np.float64)
gyroscope = np.zeros(3, dtype=np.float64)
# Battery Read Only - Analog channel 6 is battery voltage
read_analog_channels_battery = np.array([6], dtype=np.int32)
read_analog_buffer_battery = np.zeros(1, dtype=np.float64)
# Encoder Read Only - Encoder channel 0 is throttle motor encoder
read_encoder_channels_throttle = np.array([0], dtype=np.int32)
read_encoder_buffer_throttle = np.zeros(1, dtype=np.int32)
# Gyroscope Read Only - Other channels 3000:3002 are for gyroscope
read_other_channels_gyroscope = np.array([3000, 3001, 3002],
dtype=np.int32)
read_other_buffer_gyroscope = np.zeros(3, dtype=np.float64)
# Accelerometer Read Only - Other channels 4000:4002 are for accelerometer
read_other_channels_accelerometer = np.array([4000, 4001, 4002],
dtype=np.int32)
read_other_buffer_accelerometer = np.zeros(3, dtype=np.float64)
# IMU Read - Other channels 3000:3002 + 4000:4002 are for IMU
read_other_channels_IMU = np.array([3000, 3001, 3002, 4000, 4001, 4002],
dtype=np.int32)
read_other_buffer_IMU = np.zeros(6, dtype=np.float64)
# Power Read - Analog channels 5, 6 are motor current and battery voltage
read_analog_channels_power = np.array([5, 6], dtype=np.int32)
read_analog_buffer_power = np.zeros(2, dtype=np.float64)
#endregion
def __init__(self):
''' This function configures the QCar and returns a handle to the QCar card. Use the handle for other methods such as qcar_io or terminate_qcar.'''
self.card = HIL()
try:
# Open the Card
self.card.open("qcar", "0")
if self.card.is_valid():
# Set PWM mode (duty cycle) and frequency
self.card.set_pwm_mode(
np.array([0], dtype=np.uint32),
len(np.array([0], dtype=np.uint32)),
np.array([PWMMode.DUTY_CYCLE], dtype=np.int32))
self.card.set_pwm_frequency(
np.array([0], dtype=np.uint32),
len(np.array([0], dtype=np.uint32)),
np.array([60e6 / 4096], dtype=np.float64))
# Set Motor coast to 0
self.card.write_digital(
np.array([40], dtype=np.uint32),
len(np.array([40], dtype=np.uint32)),
np.zeros(len(np.array([0], dtype=np.uint32)),
dtype=np.float64))
# Set Encoder Quadrature
encoder_channels = np.array([0], dtype=np.uint32)
num_encoder_channels = len(encoder_channels)
self.card.set_encoder_quadrature_mode(
encoder_channels, num_encoder_channels,
np.array([4], dtype=np.uint32))
self.card.set_encoder_filter_frequency(
encoder_channels, num_encoder_channels,
np.array([60e6 / 1], dtype=np.uint32))
self.card.set_encoder_counts(encoder_channels,
num_encoder_channels,
np.zeros(1, dtype=np.int32))
print('QCar configured successfully.')
except HILError as h:
print(h.get_error_message())
def terminate(self):
''' This function terminates the QCar card after setting final values for throttle, steering and LEDs.'''
# PWM channel 0 is mtr cmd
pwm_channels = np.array([0], dtype=np.int32)
pwm_buffer = np.zeros(1, dtype=np.float64)
# Other channel 1000 is steering, 11008:11011 are 4x indicators, and 11000:11003 are 4x lamps
other_channels = np.array(
[1000, 11000, 11001, 11002, 11003, 11008, 11009, 11010, 11011],
dtype=np.int32)
other_buffer = np.zeros(9, dtype=np.float64)
try:
self.card.write(None, 0, pwm_channels,
len(pwm_channels), None, 0, other_channels,
len(other_channels), None, pwm_buffer, None,
other_buffer)
self.card.close()
except HILError as h:
print(h.get_error_message())
def read_encoder(self):
'''Use this to read encoder counts \n
OUTPUTS:
mtr_encoder - throttle motor encoder measurement'''
try:
if True:
#Reads: Analog Channel, Num Analog Channel, PWM Channel, Num PWM Channel, Digital Channel, Num Digital Channel, Other Channel, Num Other Channel, Analog Buffer, PWM Buffer, Digital Buffer, Other Buffer
self.card.read(None, 0, self.read_encoder_channels_throttle, 1,
None, 0, None, 0, None,
self.read_encoder_buffer_throttle, None, None)
except HILError as h:
print(h.get_error_message())
finally:
return self.read_encoder_buffer_throttle[0]
def read_gyroscope(self):
'''Use this to read the gyroscope \n
OUTPUTS:
gyroscope - gyroscopic measurement'''
try:
if True:
#Reads: Analog Channel, Num Analog Channel, PWM Channel, Num PWM Channel, Digital Channel, Num Digital Channel, Other Channel, Num Other Channel, Analog Buffer, PWM Buffer, Digital Buffer, Other Buffer
self.card.read(None, 0, None, 0, None, 0,
self.read_other_channels_gyroscope, 3, None,
None, None, self.read_other_buffer_gyroscope)
except HILError as h:
print(h.get_error_message())
finally:
self.read_other_buffer_IMU[0:3] = self.read_other_buffer_gyroscope
return self.read_other_buffer_gyroscope
def read_accelerometer(self):
'''Use this to read the accelerometer \n
OUTPUTS:
accelerometer - accelerometer measurement'''
try:
if True:
#Reads: Analog Channel, Num Analog Channel, PWM Channel, Num PWM Channel, Digital Channel, Num Digital Channel, Other Channel, Num Other Channel, Analog Buffer, PWM Buffer, Digital Buffer, Other Buffer
self.card.read(None, 0, None, 0, None, 0,
self.read_other_channels_accelerometer, 3, None,
None, None,
self.read_other_buffer_accelerometer)
except HILError as h:
print(h.get_error_message())
finally:
self.read_other_buffer_IMU[
3:6] = self.read_other_buffer_accelerometer
return self.read_other_buffer_accelerometer
def read_IMU(self):
'''Use this to read the IMU (gyroscope and accelerometer) \n
OUTPUTS:
gyroscope - gyroscopic measurement
accelerometer - accelerometer measurement'''
try:
if True:
#Reads: Analog Channel, Num Analog Channel, PWM Channel, Num PWM Channel, Digital Channel, Num Digital Channel, Other Channel, Num Other Channel, Analog Buffer, PWM Buffer, Digital Buffer, Other Buffer
self.card.read(None, 0, None, 0, None, 0,
self.read_other_channels_IMU, 6, None, None,
None, self.read_other_buffer_IMU)
except HILError as h:
print(h.get_error_message())
finally:
self.read_other_buffer_gyroscope = self.read_other_buffer_IMU[0:3]
self.read_other_buffer_accelerometer = self.read_other_buffer_IMU[
3:6]
return self.read_other_buffer_gyroscope, self.read_other_buffer_gyroscope
def read_power(self):
'''Use this to read the motor current and battery voltage \n
OUTPUTS:
mtr_current - throttle motor current measurement
bat_voltage - battery voltage measurement'''
try:
if True:
#Reads: Analog Channel, Num Analog Channel, PWM Channel, Num PWM Channel, Digital Channel, Num Digital Channel, Other Channel, Num Other Channel, Analog Buffer, PWM Buffer, Digital Buffer, Other Buffer
self.card.read(self.read_analog_channels_power, 2, None, 0,
None, 0, None, 0, self.read_analog_buffer_power,
None, None, None)
except HILError as h:
print(h.get_error_message())
finally:
self.read_analog_buffer_battery = self.read_analog_buffer_power[1]
return self.read_analog_buffer_power[
0], self.read_analog_buffer_battery
def read_std(self):
'''Use this to read the motor current, battery voltage and encoder counts \n
OUTPUTS:
mtr_current - throttle motor current measurement
bat_voltage - battery voltage measurement
mtr_encoder - throttle motor encoder measurement'''
# IO
try:
if True:
#Reads: Analog Channel, Num Analog Channel, PWM Channel, Num PWM Channel, Digital Channel, Num Digital Channel, Other Channel, Num Other Channel, Analog Buffer, PWM Buffer, Digital Buffer, Other Buffer
self.card.read(self.read_analog_channels_power, 2,
self.read_encoder_channels_throttle, 1, None, 0,
None, 0, self.read_analog_buffer_power,
self.read_encoder_buffer_throttle, None, None)
except HILError as h:
print(h.get_error_message())
finally:
self.read_analog_buffer_battery = self.read_analog_buffer_power[1]
return self.read_analog_buffer_power[
0], self.read_analog_buffer_power[
1], self.read_encoder_buffer_throttle[0]
def write_mtrs(self, mtr_cmd):
'''Use this to write motor commands\n
INPUTS:
mtr_cmd - numpy 1x2 array of throttle (%) and steering (rad) motor commands. '''
self.write_pwm_buffer_throttle[0] = -saturate(mtr_cmd[0], 0.2, -0.2)
self.write_other_buffer_steering[0] = -saturate(mtr_cmd[1], 0.5, -0.5)
try:
if True:
#Writes: Analog Channel, Num Analog Channel, PWM Channel, Num PWM Channel, Digital Channel, Num Digital Channel, Other Channel, Num Other Channel, Analog Buffer, PWM Buffer, Digital Buffer, Other Buffer
self.card.write(None, 0, self.write_pwm_channel_throttle, 1,
None, 0, self.write_other_channel_steering, 1,
None, self.write_pwm_buffer_throttle, None,
self.write_other_buffer_steering)
except HILError as h:
print(h.get_error_message())
def write_LEDs(self, LEDs):
'''Use this to write LED commands\n
INPUTS:
LEDs - numpy 1x8 array of 4x indicators (0, 1) and 4x lamps (0, 1)'''
self.write_other_buffer_LEDs = LEDs
try:
if True:
#Writes: Analog Channel, Num Analog Channel, PWM Channel, Num PWM Channel, Digital Channel, Num Digital Channel, Other Channel, Num Other Channel, Analog Buffer, PWM Buffer, Digital Buffer, Other Buffer
self.card.write(None, 0, None, 0, None, 0,
self.write_other_channels_LEDs, 8, None, None,
None, self.write_other_buffer_LEDs)
except HILError as h:
print(h.get_error_message())
def write_usr_LEDs(self, LEDs):
'''Use this to write user LED commands\n
INPUTS:
LEDs - numpy 1x4 array of 4x LEDs'''
self.write_other_buffer_usr_LEDs = LEDs
try:
if True:
#Writes: Analog Channel, Num Analog Channel, PWM Channel, Num PWM Channel, Digital Channel, Num Digital Channel, Other Channel, Num Other Channel, Analog Buffer, PWM Buffer, Digital Buffer, Other Buffer
self.card.write(None, 0, None, 0, None, 0,
self.write_other_channels_usr_LEDs, 4, None,
None, None, self.write_other_buffer_usr_LEDs)
except HILError as h:
print(h.get_error_message())
def write_std(self, mtr_cmd, LEDs):
'''Use this to write motor and LED commands\n
INPUTS:
mtr_cmd - numpy 1x2 array of throttle (%) and steering (rad) motor commands.
LEDs - numpy 1x8 array of 4x indicators (0, 1) and 4x lamps (0, 1)'''
self.write_pwm_buffer_throttle[0] = -saturate(mtr_cmd[0], 0.2, -0.2)
self.write_other_buffer_str_LEDs[0] = -saturate(mtr_cmd[1], 0.5, -0.5)
self.write_other_buffer_str_LEDs[1:9] = LEDs
try:
if True:
#Writes: Analog Channel, Num Analog Channel, PWM Channel, Num PWM Channel, Digital Channel, Num Digital Channel, Other Channel, Num Other Channel, Analog Buffer, PWM Buffer, Digital Buffer, Other Buffer
self.card.write(None, 0, self.write_pwm_channel_throttle, 1,
None, 0, self.write_other_channels_str_LEDs, 9,
None, self.write_pwm_buffer_throttle, None,
self.write_other_buffer_str_LEDs)
except HILError as h:
print(h.get_error_message())
def read_write_std(self, mtr_cmd, LEDs):
'''Use this to write motor and LED commands, and read the battery voltage, motor current and encoder counts \n
INPUTS:
mtr_cmd - numpy 1x2 array of throttle (%) and steering (rad) motor commands.
LEDs - numpy 1x8 array of 4x indicators (0, 1) and 4x lamps (0, 1)
OUTPUTS:
mtr_current - throttle motor current measurement
bat_voltage - battery voltage measurement
mtr_encoder - throttle motor encoder measurement'''
self.write_pwm_buffer_throttle[0] = -saturate(mtr_cmd[0], 0.2, -0.2)
self.write_other_buffer_str_LEDs[0] = -saturate(mtr_cmd[1], 0.5, -0.5)
self.write_other_buffer_str_LEDs[1:9] = LEDs
# IO
try:
if True:
#Writes: Analog Channel, Num Analog Channel, PWM Channel, Num PWM Channel, Digital Channel, Num Digital Channel, Other Channel, Num Other Channel, Analog Buffer, PWM Buffer, Digital Buffer, Other Buffer
self.card.write(None, 0, self.write_pwm_channel_throttle, 1,
None, 0, self.write_other_channels_str_LEDs, 9,
None, self.write_pwm_buffer_throttle, None,
self.write_other_buffer_str_LEDs)
#Reads: Analog Channel, Num Analog Channel, PWM Channel, Num PWM Channel, Digital Channel, Num Digital Channel, Other Channel, Num Other Channel, Analog Buffer, PWM Buffer, Digital Buffer, Other Buffer
self.card.read(self.read_analog_channels_power, 2,
self.read_encoder_channels_throttle, 1, None, 0,
None, 0, self.read_analog_buffer_power,
self.read_encoder_buffer_throttle, None, None)
except HILError as h:
print(h.get_error_message())
finally:
self.read_analog_buffer_battery = self.read_analog_buffer_power[1]
return self.read_analog_buffer_power[
0], self.read_analog_buffer_battery, self.read_encoder_buffer_throttle[
0]
class QBot2e:
# Define class-level variables
_card = None
_capture = None
_RGB_stream = None
_ai_channels = None
_ai_buffer = None
_di_channels = None
_di_buffer = None
_do_channels = None
_do_buffer = None
_oi_channels = None
_oi_buffer = None
_gyro_z_bias = None
_oo_channels = None
_oo_buffer = None
_enc_channels = None
_enc_buffer = None
_image_data = None
_qbot_diameter = 0.235
# Initilize QBot2e
def __init__(self):
print("Initializing QBot2e...")
# Define DAQ type
self._card = HIL()
self._card.open("qbot2e", "0")
# Configure analog channels
self._ai_channels = array('I', [0, 4, 5])
self._ai_buffer = array('d', [0.0] * len(self._ai_channels))
# Configure digital channels
self._do_channels = array('I', [i for i in range(28, 36)])
self._do_buffer = array('b', [0, 0, 0, 0, 1, 1, 1, 1])
self._di_channels = array('I', [i for i in range(28, 59)])
self._di_buffer = array('b', [0] * len(self._di_channels))
#self._card.set_digital_directions(self._di_channels, self._di_num_channels, self._do_channels, self._do_num_channels)
# Configure other channels
self._oo_channels = array('I', [2000, 2001])
self._oo_buffer = array('d', [0.0] * len(self._oo_channels))
self._oi_channels = array(
'I', [1002, 3000, 3001, 3002, 11000, 11001, 12000, 16000])
self._oi_buffer = array('d', [0.0] * len(self._oi_channels))
# Configure encoder channels
self._enc_channels = array('I', [0, 1])
self._enc_buffer = array('i', [0] * len(self._enc_channels))
# Reset QBot2e
self.reset()
print("QBot2e Initialized")
################## ANALOG IN METHODS ##################
# Get analog input buffers
def update_ai_buffer(self):
self._card.read_analog(self._ai_channels, len(self._ai_channels),
self._ai_buffer)
# Read battery voltage
def get_batt_volts(self):
self.update_ai_buffer()
return self._ai_buffer[0]
################## ENCODER METHODS ####################
# Get/set encoder values
def update_enc_buffer(self):
self._card.read_encoder(self._enc_channels, len(self._enc_channels),
self._enc_buffer)
def push_enc_buffer(self):
self._card.set_encoder_counts(self._enc_channels,
len(self._enc_channels),
self._enc_buffer)
# Read raw encoder counts
def read_encoder_count(self, _channel):
self.update_enc_buffer()
return self._enc_buffer[_channel]
################## DIGITAL I/O METHODS ################
# Get/set digital I/O buffers
def update_di_buffer(self):
self._card.read_digital(self._di_channels, len(self._di_channels),
self._di_buffer)
def push_do_buffer(self):
self._card.write_digital(self._do_channels, len(self._do_channels),
self._do_buffer)
# Set LED outputs
def set_leds(self, bLED=[0, 0, 0, 0]):
self._do_buffer[0:4] = array('b', bLED)
self.push_do_buffer()
# Read all digital in
def read_din(self):
self.update_di_buffer()
return self._di_buffer
# Return bump sensor inputs
def read_bump_sensors(self):
self.update_di_buffer()
return self._di_buffer[0:3]
# Return button inputs
def read_buttons(self):
self.update_di_buffer()
return self._di_buffer[8:11]
# Read right dock IR; [NR, NC, NL, FR, FC, FL]
def read_right_dock_ir(self):
self.update_di_buffer()
return self._di_buffer[13:19]
# Read center dock IR; [NR, NC, NL, FR, FC, FL]
def read_center_dock_ir(self):
self.update_di_buffer()
return self._di_buffer[19:25]
# Read left dock IR; [NR, NC, NL, FR, FC, FL]
def read_left_dock_ir(self):
self.update_di_buffer()
return self._di_buffer[25:31]
################## OTHER I/O METHODS ##################
# Get/set other I/O buffers
def update_oi_buffer(self):
self._card.read_other(self._oi_channels, len(self._oi_channels),
self._oi_buffer)
def push_oo_buffer(self):
self._card.write_other(self._oo_channels, len(self._oo_channels),
self._oo_buffer)
# Read z gyro
def read_z_gyro(self):
self.update_oi_buffer()
z_gyro = self._oi_buffer[3] - self._z_bias
return z_gyro
# Measure gyro z-axis bias
def update_gyro_z_bias(self):
gyro = 0.0
for i in range(10000):
self.update_oi_buffer()
gyro += self._oi_buffer[3]
self._gyro_z_bias = gyro / 10000.0
print("Z-axis gyro bias: {}".format(self._gyro_z_bias))
# Set motor speed command
def set_velocity(self, velocity=[0, 0]):
self._oo_buffer = array('d', velocity)
self.push_oo_buffer()
################## CONTROL METHODS ####################
# Move a set distance and angle open loop
def move_time(self, distance=0, angle=0, move_time=1):
# Ignore command if time <= 0 seconds
if (move_time > 0):
avg_linear_velocity = distance / move_time
avg_angular_velocity = angle / move_time
linear_velocity_delta = avg_angular_velocity * (
self._qbot_diameter / 2)
velocity_command = [
avg_linear_velocity + linear_velocity_delta,
avg_linear_velocity - linear_velocity_delta
]
self.set_velocity(velocity_command)
time.sleep(move_time)
self.halt()
# Move a set distance and angle using odometry
def move_odo(self, distance=0, angle=0, move_time=1):
# TODO
time.sleep(move_time)
# Move a set distance and angle using gyro
def move_gyro(self, distance=0, angle=0, move_time=1):
# TODO
time.sleep(move_time)
# Stop both motors
def halt(self):
self.set_velocity([0, 0])
################## DEVICE MANAGEMENT ##################
# Reset QBot2e
def reset(self):
print("Resetting QBot2e...")
#Clear LEDs
self.set_leds([0, 0, 0, 0])
# Stop motors
self.set_velocity([0, 0])
# Reset encoders to 0
self._enc_buffer = array('I', [0, 0])
self.push_enc_buffer()
# Sample gyroscope z-axis bias
if self._gyro_z_bias == None:
self.update_gyro_z_bias()
# Close DAQ
def close(self):
self.__exit__()
# Exit routine
def __exit__(self):
print("Closing DAQ...")
self.reset()
self._card.close()
print("QBot2e closed")
class q2usb:
# Define class-level variables
# Q2-USB variables
_card = None
_ai_channels = None
_ai_buffer = None
_ao_channels = None
_ao_buffer = None
_enc_channels = None
_enc_buffer = None
# Initilize Q2-USB
def __init__(self):
# Define DAQ type as Q2-USB
self._card = HIL()
self._card.open("q2_usb", "0")
# Set Q2-USB speed to noraml
self._card.set_card_specific_options("update_rate=normal;", 50)
# Configure analog input channels
self._ai_channels = array('I', [0, 1])
self._ai_buffer = array('d', [0.0] * len(self._ai_channels))
# Configure analog output channels
self._ao_channels = array('I', [0, 1])
self._ao_buffer = array('d', [0.0] * len(self._ao_channels))
# Configure encoder input channels
self._enc_channels = array('I', [0, 1])
self._enc_buffer = array('i', [0] * len(self._enc_channels))
# Write 0 V to all AO channels
self._card.write_analog(self._ao_channels, len(self._ao_channels),
self._ao_buffer)
# Set encoder counts to 0
_init_enc_counts = array('i', [0, 0])
self._card.set_encoder_counts(self._enc_channels,
len(self._enc_channels),
_init_enc_counts)
print("Q2-USB DAQ Initialized")
######################### Q2-USB ANALOG IN METHODS #########################
# Read single analog input channel
def read_analog_input(self, channel):
self._card.read_analog(self._ai_channels, len(self._ai_channels),
self._ai_buffer)
return self._ai_buffer[channel]
######################### Q2-USB ANALOG OUT METHODS #########################
# Write value to single analog output channel
def write_analog_output(self, channel, value):
self._card.write_analog(array('I', [channel]), 1, array('d', [value]))
######################### Q2-USB ENCODER METHODS #########################
# Read raw encoder counts for single channel
def read_encoder_count(self, channel):
self._card.read_encoder(self._enc_channels, len(self._enc_channels),
self._enc_buffer)
return self._enc_buffer[channel]
################## DEVICE MANAGEMENT ##################
# Close Q2-USB
def close(self):
self.__exit__()
# Exit routine
def __exit__(self):
print("Closing Q2-USB ...")
# Write 0 V AO channels 0 and 1
self.write_analog_output(0, 0)
self.write_analog_output(1, 0)
# Close DAQ
self._card.close()
print("Q2-USB closed")