def __init__(self, experiment_info, enabled=True, record_to_file=True):
super(Prototype2bridge, self).__init__(enabled)
self.factory = DeviceFactory()
self.prototype2_bridge_arduino = Arduino("prototype2", self.factory)
self.experiment_info = experiment_info
self.timestamp_sum = 0.0
self.num_packets = 0
self.record_to_file = record_to_file
def __init__(self, factory, enabled=True, enable_reporting=True):
self.set_logger(write=True)
super(EncoderReaderBridge, self).__init__(enabled)
self.factory = factory
self.encoder_reader_bridge_arduino = Arduino("encoder_reader",
self.factory)
self.prev_broadcast_time = 0.0
self.prev_report_time = 0.0
self.enable_reporting = enable_reporting
self.num_packets_received = 0
def __init__(self, factory, enabled=True, enable_reporting=True):
self.set_logger(write=True)
super(BrakeControllerBridge, self).__init__(enabled)
self.factory = factory
self.brake_controller_bridge_arduino = Arduino("brake_controller",
self.factory)
self.prev_broadcast_time = 0.0
self.prev_report_time = 0.0
self.enable_reporting = enable_reporting
self.kp = 0.0
self.ki = 0.0
self.kd = 0.0
import time
from arduino_factory import Arduino, DeviceFactory
factory = DeviceFactory()
test = Arduino("test", factory)
first_packet = test.start()
print("first packet:", first_packet)
try:
while factory.ok():
packet = test.read()
if packet.name == "numbers":
if packet.data[0] >= 12:
test.write("d")
test.write("f")
test.write("s1")
test.write("s2")
current_time = time.time()
print("local: %0.4f, arduino: %0.4f, diff: %0.4f, data: %s" %
(current_time - test.start_time, packet.timestamp -
test.start_time, current_time - packet.timestamp, packet.data))
except KeyboardInterrupt:
pass
except BaseException:
raise
finally:
factory.stop_all()
class BrakeControllerBridge(Node):
def __init__(self, factory, enabled=True, enable_reporting=True):
self.set_logger(write=True)
super(BrakeControllerBridge, self).__init__(enabled)
self.factory = factory
self.brake_controller_bridge_arduino = Arduino("brake_controller",
self.factory)
self.prev_broadcast_time = 0.0
self.prev_report_time = 0.0
self.enable_reporting = enable_reporting
self.kp = 0.0
self.ki = 0.0
self.kd = 0.0
async def setup(self):
start_packet = self.brake_controller_bridge_arduino.start()
self.kp = start_packet.data[0]
self.ki = start_packet.data[1]
self.kd = start_packet.data[2]
self.prev_broadcast_time = time.time()
self.prev_report_time = time.time()
self.logger.debug("start_packet: '%s'" % (str(start_packet)))
async def loop(self):
while self.factory.ok():
packet = self.brake_controller_bridge_arduino.read()
self.logger.debug("packet: '%s'" % (str(packet)))
await asyncio.sleep(0.0)
if packet.name is None:
self.logger.warning("No packets found!")
elif packet.name == "brake":
# shunt_voltage = packet.data[0]
# bus_voltage = packet.data[1]
# current_mA = packet.data[2]
# power_mW = packet.data[3]
# load_voltage = packet.data[4]
# current_pin_value = packet.data[5]
# set_point = packet.data[6]
self.log_to_buffer(packet.receive_time, packet)
# if time.time() - self.prev_broadcast_time > 0.25:
if self.enable_reporting and time.time(
) - self.prev_report_time > 1.0:
try:
print("shunt voltage (V): %0.2f\n"
"bus voltage (V): %0.2f\n"
"current (mA): %0.2f\n"
"power (mW): %0.2f\n"
"load voltage (V): %0.2f\n"
"pwm pin value: %s\n"
"set point (mA): %0.2f\n" % tuple(packet.data))
except TypeError as error:
print(
"The brake controller bridge encountered a formatting error while reporting values:"
)
print(error)
self.prev_report_time = time.time()
self.prev_broadcast_time = time.time()
await self.broadcast(packet)
def command_brake(self, command):
self.brake_controller_bridge_arduino.write("b" + str(float(command)))
def set_kp(self, kp):
self.kp = kp
self.brake_controller_bridge_arduino.write("kp" + str(float(kp)))
def set_ki(self, ki):
self.ki = ki
self.brake_controller_bridge_arduino.write("ki" + str(float(ki)))
def set_kd(self, kd):
self.kd = kd
self.brake_controller_bridge_arduino.write("kd" + str(float(kd)))
async def teardown(self):
self.factory.stop_all()
class Prototype2bridge(Node):
def __init__(self, experiment_info, enabled=True, record_to_file=True):
super(Prototype2bridge, self).__init__(enabled)
self.factory = DeviceFactory()
self.prototype2_bridge_arduino = Arduino("prototype2", self.factory)
self.experiment_info = experiment_info
self.timestamp_sum = 0.0
self.num_packets = 0
self.record_to_file = record_to_file
async def setup(self):
# start_packet = self.prototype2_bridge_arduino.start()
self.prototype2_bridge_arduino.start()
async def loop(self):
prev_brake_val = 0
cycle_num = 1
initial_enc_recorded = False
if self.record_to_file:
print("Cycle #%s of %s" %
(cycle_num, self.experiment_info.repeats * 2))
while self.factory.ok():
packet = self.prototype2_bridge_arduino.read()
await asyncio.sleep(0.0)
if packet.name is None:
self.logger.warning("No packets found!")
elif packet.name == "enc":
encoder1_deg = packet.data[0]
encoder2_deg = packet.data[1]
self.timestamp_sum += packet.timestamp
self.num_packets += 1
if self.record_to_file:
if not initial_enc_recorded:
# self.experiment_info.record_encoder_start_vals(packet.timestamp, encoder1_deg, encoder2_deg)
# initial_enc_recorded = True
if packet.timestamp >= 2.0:
self.experiment_info.record_encoder_start_vals(
packet.timestamp, encoder1_deg, encoder2_deg)
initial_enc_recorded = True
else:
continue
self.experiment_info.record_encoders(packet.timestamp,
encoder1_deg,
encoder2_deg)
await self.broadcast(packet)
elif packet.name == "brake":
brake_val = packet.data[0]
self.experiment_info.record_torque_command(
packet.timestamp, brake_val)
print("Torque value '%s' processed" % brake_val)
if brake_val != prev_brake_val:
prev_brake_val = brake_val
if brake_val == 0:
cycle_num += 1
if cycle_num > self.experiment_info.repeats * 2:
print("Experiment done")
return
if self.record_to_file:
print(
"Cycle #%s of %s" %
(cycle_num, self.experiment_info.repeats * 2))
elif packet.name == "motor":
motor_val = packet.data[0]
self.experiment_info.record_motor_command(
packet.timestamp, motor_val)
print("Motor speed '%s' processed" % motor_val)
def generate_experiment(self, command_interval, time_interval,
motor_command, repeats, max_torque_command):
self.command_motor(motor_command)
self.prototype2_bridge_arduino.write_pause(
5) # wait for twist to settle
for _ in range(2): # command in both directions
for _ in range(repeats):
self.command_rise(max_torque_command, command_interval,
time_interval)
self.command_brake(max_torque_command)
self.prototype2_bridge_arduino.write_pause(5)
self.command_fall(max_torque_command, command_interval,
time_interval)
self.command_brake(0)
self.prototype2_bridge_arduino.write_pause(
5) # ensure brake dynamics reset
self.command_motor(-motor_command)
self.prototype2_bridge_arduino.write_pause(time_interval)
self.command_motor(0)
def command_rise(self, max_torque_command, command_interval,
time_interval):
self.command_brake(0)
brake_command = 0
for brake_command in range(0, max_torque_command + 1,
command_interval):
self.command_brake(brake_command)
self.prototype2_bridge_arduino.write_pause(time_interval)
if brake_command != max_torque_command:
self.command_brake(max_torque_command)
self.prototype2_bridge_arduino.write_pause(time_interval)
def command_fall(self, max_torque_command, command_interval,
time_interval):
self.command_brake(max_torque_command)
brake_command = 0
for brake_command in range(max_torque_command, -1, -command_interval):
self.command_brake(brake_command)
self.prototype2_bridge_arduino.write_pause(time_interval)
if brake_command != 0:
self.command_brake(0)
self.prototype2_bridge_arduino.write_pause(time_interval)
def command_brake(self, command):
self.prototype2_bridge_arduino.write("b" + str(int(command)))
def command_motor(self, command):
self.prototype2_bridge_arduino.write("m" + str(int(command)))
async def teardown(self):
self.factory.stop_all()
if self.record_to_file:
self.experiment_info.write_experiment_to_file()
print("time fps avg: %0.4f" % (self.timestamp_sum / self.num_packets))
class EncoderReaderBridge(Node):
def __init__(self, factory, enabled=True, enable_reporting=True):
self.set_logger(write=True)
super(EncoderReaderBridge, self).__init__(enabled)
self.factory = factory
self.encoder_reader_bridge_arduino = Arduino("encoder_reader",
self.factory)
self.prev_broadcast_time = 0.0
self.prev_report_time = 0.0
self.enable_reporting = enable_reporting
self.num_packets_received = 0
async def setup(self):
start_packet = self.encoder_reader_bridge_arduino.start()
# self.initial_abs_enc1 = -start_packet.data[0]
# self.initial_abs_enc2 = -start_packet.data[1]
self.prev_broadcast_time = time.time()
self.prev_report_time = time.time()
async def loop(self):
while self.factory.ok():
time_diff = 0.0
packet = None
while time_diff == 0.0 or time_diff > 0.1: # don't let the packets get behind
packet = self.encoder_reader_bridge_arduino.read()
self.log_to_buffer(packet.receive_time, packet)
self.num_packets_received += 1
time_diff = time.time() - packet.receive_time
await asyncio.sleep(0.0)
if packet.name is None:
self.logger.warning("No packets found!")
elif packet.name == "enc":
# abs_enc1_angle = packet.data[0]
# abs_enc2_angle = packet.data[1]
# abs_enc1_analog = packet.data[2]
# abs_enc2_analog = packet.data[3]
# enc1_pos = packet.data[4]
# enc2_pos = packet.data[5]
# motor_pos = packet.data[6]
# if time.time() - self.prev_broadcast_time > 0.25:
if self.enable_reporting and time.time(
) - self.prev_report_time > 1.0:
try:
print("abs_enc1_angle: %0.6f\n"
"abs_enc2_angle: %0.6f\n"
"abs_enc1_analog: %d\n"
"abs_enc2_analog: %d\n"
"enc1_pos: %0.6f\n"
"enc2_pos: %0.6f\n"
"motor_pos: %0.6f" % tuple(packet.data))
print()
except TypeError as error:
print(
"The encoder reader bridge encountered a formatting error while reporting values:"
)
print(error)
self.prev_report_time = time.time()
self.prev_broadcast_time = time.time()
await self.broadcast(packet)
async def teardown(self):
self.factory.stop_all()
self.logger.info("packets per sec: %s" %
(self.num_packets_received /
(time.time() - self.start_time)))